Unify cost function differentiation benchmarks Replace internal/ceres/autodiff_benchmarks/ with internal/ceres/benchmark_cost_functions.h and internal/ceres/cost_function_differentiation_benchmark.cc: - Consolidate the benchmark cost functors into internal/ceres/benchmark_cost_functions.h as self-contained structs inheriting from BenchmarkCostFunctor<kNumResiduals, Ns...> with default constructors and MakeParameters(), and remove the internal/ceres/autodiff_benchmarks/ subdirectory. - Add nine real-world camera projection, epipolar, and SLAM cost functors (OpenCV, FullOpenCV, Rig, Sampson, Fisheye, and Spherical from COLMAP; Brown-Conrady and Nuke InvertIntrinsics from Blender libmv; and PoseGraph3dError from Ceres). - Benchmark AutoDiffCostFunction (static and dynamic residuals), DynamicAutoDiffCostFunction, NumericDiffCostFunction, and DynamicNumericDiffCostFunction (FORWARD, CENTRAL, RIDDERS) using a single BM_CostFunction<CostFunctor, kDiffType, kEvalType> template and CostFunctionFactory in cost_function_differentiation_benchmark.cc. - Add CostFunctionToFunctor and DynamicCostFunctionToFunctor benchmarks. - Fix ConstantCostFunction to pass kParameterBlockSize instead of 1 and remove redundant AnalyticConstantCostFunction. - Move -mllvm -inlinehint-threshold=1000000 into add_dependencies_to_benchmark in internal/ceres/CMakeLists.txt. Change-Id: Ic02f4d29a15b5faa606a5af033c19d0bfd1bba91
diff --git a/BUILD b/BUILD index a2d07a0..ac8ff91 100644 --- a/BUILD +++ b/BUILD
@@ -213,10 +213,11 @@ ]] cc_binary( - name = "autodiff_benchmarks", - srcs = ["internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc"] + glob([ - "internal/ceres/autodiff_benchmarks/*.h", - ]), + name = "cost_function_differentiation_benchmark", + srcs = [ + "internal/ceres/benchmark_cost_functions.h", + "internal/ceres/cost_function_differentiation_benchmark.cc", + ], copts = ["-mllvm -inlinehint-threshold=1000000"], deps = TEST_DEPS + ["@google_benchmark//:benchmark"], )
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 286f353..f85d0d6 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -561,5 +561,7 @@ block_jacobi_preconditioner_benchmark.cc) add_dependencies_to_benchmark(block_jacobi_preconditioner_benchmark) - add_subdirectory(autodiff_benchmarks) + add_executable(cost_function_differentiation_benchmark + cost_function_differentiation_benchmark.cc) + add_dependencies_to_benchmark(cost_function_differentiation_benchmark) endif (_Ceres_FEATURE_BENCHMARKS)
diff --git a/internal/ceres/autodiff_benchmarks/CMakeLists.txt b/internal/ceres/autodiff_benchmarks/CMakeLists.txt deleted file mode 100644 index 99af152..0000000 --- a/internal/ceres/autodiff_benchmarks/CMakeLists.txt +++ /dev/null
@@ -1,11 +0,0 @@ -# TODO: Add support for other compilers -if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - # Increase the inlining threshold only for those functions marked with an - # inline hint. This is typically far more realistic to significantly increase - # in a large code-base than -inline-threshold as that has a larger scope. - list(APPEND CERES_BENCHMARK_FLAGS "-mllvm" "-inlinehint-threshold=1000000") -endif() - -add_executable(autodiff_benchmarks autodiff_benchmarks.cc) -add_dependencies_to_benchmark(autodiff_benchmarks) -target_compile_options(autodiff_benchmarks PRIVATE ${CERES_BENCHMARK_FLAGS})
diff --git a/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc b/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc deleted file mode 100644 index 87e0067..0000000 --- a/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc +++ /dev/null
@@ -1,430 +0,0 @@ -// 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) - -#include <memory> -#include <random> -#include <utility> - -#include "benchmark/benchmark.h" -#include "ceres/autodiff_benchmarks/brdf_cost_function.h" -#include "ceres/autodiff_benchmarks/constant_cost_function.h" -#include "ceres/autodiff_benchmarks/linear_cost_functions.h" -#include "ceres/autodiff_benchmarks/photometric_error.h" -#include "ceres/autodiff_benchmarks/relative_pose_error.h" -#include "ceres/autodiff_benchmarks/snavely_reprojection_error.h" -#include "ceres/ceres.h" - -namespace ceres { - -enum Dynamic { kNotDynamic, kDynamic }; - -// Transforms a static functor into a dynamic one. -template <typename CostFunctionType, int kNumParameterBlocks> -class ToDynamic { - public: - template <typename... _Args> - explicit ToDynamic(_Args&&... __args) - : cost_function_(std::forward<_Args>(__args)...) {} - - template <typename T> - bool operator()(const T* const* parameters, T* residuals) const { - return Apply( - parameters, residuals, std::make_index_sequence<kNumParameterBlocks>()); - } - - private: - template <typename T, size_t... Indices> - bool Apply(const T* const* parameters, - T* residuals, - std::index_sequence<Indices...>) const { - return cost_function_(parameters[Indices]..., residuals); - } - - CostFunctionType cost_function_; -}; - -template <int kParameterBlockSize> -static void BM_ConstantAnalytic(benchmark::State& state) { - constexpr int num_residuals = 1; - std::array<double, kParameterBlockSize> parameters_values; - std::iota(parameters_values.begin(), parameters_values.end(), 0); - double* parameters[] = {parameters_values.data()}; - - std::array<double, num_residuals> residuals; - - std::array<double, num_residuals * kParameterBlockSize> jacobian_values; - double* jacobians[] = {jacobian_values.data()}; - - std::unique_ptr<ceres::CostFunction> cost_function( - new AnalyticConstantCostFunction<kParameterBlockSize>()); - - for (auto _ : state) { - cost_function->Evaluate(parameters, residuals.data(), jacobians); - } -} - -// Helpers for CostFunctionFactory. -template <typename DynamicCostFunctionType> -void AddParameterBlocks(DynamicCostFunctionType*) {} - -template <int HeadN, int... TailNs, typename DynamicCostFunctionType> -void AddParameterBlocks(DynamicCostFunctionType* dynamic_function) { - dynamic_function->AddParameterBlock(HeadN); - AddParameterBlocks<TailNs...>(dynamic_function); -} - -// Creates an autodiff cost function wrapping `CostFunctor`, with -// `kNumResiduals` residuals and parameter blocks with sized `Ns..`. -// Depending on `kIsDynamic`, either a static or dynamic cost function is -// created. -// `args` are forwarded to the `CostFunctor` constructor. -template <Dynamic kIsDynamic> -struct CostFunctionFactory {}; - -template <> -struct CostFunctionFactory<kNotDynamic> { - template <typename CostFunctor, - int kNumResiduals, - int... Ns, - typename... Args> - static std::unique_ptr<ceres::CostFunction> Create(Args&&... args) { - return std::make_unique< - ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns...>>( - new CostFunctor(std::forward<Args>(args)...)); - } -}; - -template <> -struct CostFunctionFactory<kDynamic> { - template <typename CostFunctor, - int kNumResiduals, - int... Ns, - typename... Args> - static std::unique_ptr<ceres::CostFunction> Create(Args&&... args) { - constexpr const int kNumParameterBlocks = sizeof...(Ns); - auto dynamic_function = std::make_unique<ceres::DynamicAutoDiffCostFunction< - ToDynamic<CostFunctor, kNumParameterBlocks>>>( - new ToDynamic<CostFunctor, kNumParameterBlocks>( - std::forward<Args>(args)...)); - dynamic_function->SetNumResiduals(kNumResiduals); - AddParameterBlocks<Ns...>(dynamic_function.get()); - return dynamic_function; - } -}; - -template <int kParameterBlockSize, Dynamic kIsDynamic> -static void BM_ConstantAutodiff(benchmark::State& state) { - constexpr int num_residuals = 1; - std::array<double, kParameterBlockSize> parameters_values; - std::iota(parameters_values.begin(), parameters_values.end(), 0); - double* parameters[] = {parameters_values.data()}; - - std::array<double, num_residuals> residuals; - - std::array<double, num_residuals * kParameterBlockSize> jacobian_values; - double* jacobians[] = {jacobian_values.data()}; - - std::unique_ptr<ceres::CostFunction> cost_function = - CostFunctionFactory<kIsDynamic>:: - template Create<ConstantCostFunction<kParameterBlockSize>, 1, 1>(); - - for (auto _ : state) { - cost_function->Evaluate(parameters, residuals.data(), jacobians); - } -} - -BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 1); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 1, kNotDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 1, kDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 10); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 10, kNotDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 10, kDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 20); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 20, kNotDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 20, kDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 30); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 30, kNotDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 30, kDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 40); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 40, kNotDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 40, kDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 50); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 50, kNotDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 50, kDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAnalytic, 60); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 60, kNotDynamic); -BENCHMARK_TEMPLATE(BM_ConstantAutodiff, 60, kDynamic); - -template <Dynamic kIsDynamic> -static void BM_Linear1AutoDiff(benchmark::State& state) { - double parameter_block1[] = {1.}; - double* parameters[] = {parameter_block1}; - - double jacobian1[1]; - double residuals[1]; - double* jacobians[] = {jacobian1}; - - std::unique_ptr<ceres::CostFunction> cost_function = CostFunctionFactory< - kIsDynamic>::template Create<Linear1CostFunction, 1, 1>(); - - for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); - } -} -BENCHMARK_TEMPLATE(BM_Linear1AutoDiff, kNotDynamic)->Arg(0)->Arg(1); -BENCHMARK_TEMPLATE(BM_Linear1AutoDiff, kDynamic)->Arg(0)->Arg(1); - -template <Dynamic kIsDynamic> -static void BM_Linear10AutoDiff(benchmark::State& state) { - double parameter_block1[] = {1., 2., 3., 4., 5., 6., 7., 8., 9., 10.}; - double* parameters[] = {parameter_block1}; - - double jacobian1[10 * 10]; - double residuals[10]; - double* jacobians[] = {jacobian1}; - - std::unique_ptr<ceres::CostFunction> cost_function = CostFunctionFactory< - kIsDynamic>::template Create<Linear10CostFunction, 10, 10>(); - - for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); - } -} -BENCHMARK_TEMPLATE(BM_Linear10AutoDiff, kNotDynamic)->Arg(0)->Arg(1); -BENCHMARK_TEMPLATE(BM_Linear10AutoDiff, kDynamic)->Arg(0)->Arg(1); - -// From the NIST problem collection. -struct Rat43CostFunctor { - Rat43CostFunctor(const double x, const double y) : x_(x), y_(y) {} - - template <typename T> - inline bool operator()(const T* parameters, T* residuals) const { - const T& b1 = parameters[0]; - const T& b2 = parameters[1]; - const T& b3 = parameters[2]; - const T& b4 = parameters[3]; - residuals[0] = b1 * pow(1.0 + exp(b2 - b3 * x_), -1.0 / b4) - y_; - return true; - } - - static constexpr int kNumParameterBlocks = 1; - - private: - const double x_; - const double y_; -}; - -template <Dynamic kIsDynamic> -static void BM_Rat43AutoDiff(benchmark::State& state) { - double parameter_block1[] = {1., 2., 3., 4.}; - double* parameters[] = {parameter_block1}; - - double jacobian1[] = {0.0, 0.0, 0.0, 0.0}; - double residuals; - double* jacobians[] = {jacobian1}; - const double x = 0.2; - const double y = 0.3; - std::unique_ptr<ceres::CostFunction> cost_function = - CostFunctionFactory<kIsDynamic>::template Create<Rat43CostFunctor, 1, 4>( - x, y); - - for (auto _ : state) { - cost_function->Evaluate( - parameters, &residuals, state.range(0) ? jacobians : nullptr); - } -} -BENCHMARK_TEMPLATE(BM_Rat43AutoDiff, kNotDynamic)->Arg(0)->Arg(1); -BENCHMARK_TEMPLATE(BM_Rat43AutoDiff, kDynamic)->Arg(0)->Arg(1); - -template <Dynamic kIsDynamic> -static void BM_SnavelyReprojectionAutoDiff(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 = CostFunctionFactory< - kIsDynamic>::template Create<SnavelyReprojectionError, 2, 9, 3>(x, y); - - for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); - } -} - -BENCHMARK_TEMPLATE(BM_SnavelyReprojectionAutoDiff, kNotDynamic)->Arg(0)->Arg(1); -BENCHMARK_TEMPLATE(BM_SnavelyReprojectionAutoDiff, kDynamic)->Arg(0)->Arg(1); - -template <Dynamic kIsDynamic> -static void BM_PhotometricAutoDiff(benchmark::State& state) { - constexpr int PATCH_SIZE = 8; - - using FunctorType = PhotometricError<PATCH_SIZE>; - using ImageType = Eigen::Matrix<uint8_t, 128, 128, Eigen::RowMajor>; - - // Prepare parameter / residual / jacobian blocks. - double parameter_block1[] = {1., 2., 3., 4., 5., 6., 7.}; - double parameter_block2[] = {1.1, 2.1, 3.1, 4.1, 5.1, 6.1, 7.1}; - double parameter_block3[] = {1.}; - double* parameters[] = {parameter_block1, parameter_block2, parameter_block3}; - - Eigen::Map<Eigen::Quaterniond>(parameter_block1).normalize(); - Eigen::Map<Eigen::Quaterniond>(parameter_block2).normalize(); - - double jacobian1[FunctorType::PATCH_SIZE * FunctorType::POSE_SIZE]; - double jacobian2[FunctorType::PATCH_SIZE * FunctorType::POSE_SIZE]; - double jacobian3[FunctorType::PATCH_SIZE * FunctorType::POINT_SIZE]; - double residuals[FunctorType::PATCH_SIZE]; - double* jacobians[] = {jacobian1, jacobian2, jacobian3}; - - // Prepare data (fixed seed for repeatability). - std::mt19937::result_type seed = 42; - std::mt19937 gen(seed); - std::uniform_real_distribution<double> uniform01(0.0, 1.0); - std::uniform_int_distribution<unsigned int> uniform0255(0, 255); - - FunctorType::Patch<double> intensities_host = - FunctorType::Patch<double>::NullaryExpr( - [&]() { return uniform0255(gen); }); - - // Set bearing vector's z component to 1, i.e. pointing away from the camera, - // to ensure they are (likely) in the domain of the projection function (given - // a small rotation between host and target frame). - FunctorType::PatchVectors<double> bearings_host = - FunctorType::PatchVectors<double>::NullaryExpr( - [&]() { return uniform01(gen); }); - bearings_host.row(2).array() = 1; - bearings_host.colwise().normalize(); - - ImageType image = ImageType::NullaryExpr( - [&]() { return static_cast<uint8_t>(uniform0255(gen)); }); - FunctorType::Grid grid(image.data(), 0, image.rows(), 0, image.cols()); - FunctorType::Interpolator image_target(grid); - - FunctorType::Intrinsics intrinsics; - intrinsics << 128, 128, 1, -1, 0.5, 0.5; - - std::unique_ptr<ceres::CostFunction> cost_function = - CostFunctionFactory<kIsDynamic>::template Create<FunctorType, - FunctorType::PATCH_SIZE, - FunctorType::POSE_SIZE, - FunctorType::POSE_SIZE, - FunctorType::POINT_SIZE>( - intensities_host, bearings_host, image_target, intrinsics); - - for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); - } -} - -BENCHMARK_TEMPLATE(BM_PhotometricAutoDiff, kNotDynamic)->Arg(0)->Arg(1); -BENCHMARK_TEMPLATE(BM_PhotometricAutoDiff, kDynamic)->Arg(0)->Arg(1); - -template <Dynamic kIsDynamic> -static void BM_RelativePoseAutoDiff(benchmark::State& state) { - using FunctorType = RelativePoseError; - - double parameter_block1[] = {1., 2., 3., 4., 5., 6., 7.}; - double parameter_block2[] = {1.1, 2.1, 3.1, 4.1, 5.1, 6.1, 7.1}; - double* parameters[] = {parameter_block1, parameter_block2}; - - Eigen::Map<Eigen::Quaterniond>(parameter_block1).normalize(); - Eigen::Map<Eigen::Quaterniond>(parameter_block2).normalize(); - - double jacobian1[6 * 7]; - double jacobian2[6 * 7]; - double residuals[6]; - double* jacobians[] = {jacobian1, jacobian2}; - - Eigen::Quaterniond q_i_j = Eigen::Quaterniond(1, 2, 3, 4).normalized(); - Eigen::Vector3d t_i_j(1, 2, 3); - - std::unique_ptr<ceres::CostFunction> cost_function = - CostFunctionFactory<kIsDynamic>::template Create<FunctorType, 6, 7, 7>( - q_i_j, t_i_j); - - for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); - } -} - -BENCHMARK_TEMPLATE(BM_RelativePoseAutoDiff, kNotDynamic)->Arg(0)->Arg(1); -BENCHMARK_TEMPLATE(BM_RelativePoseAutoDiff, kDynamic)->Arg(0)->Arg(1); - -template <Dynamic kIsDynamic> -static void BM_BrdfAutoDiff(benchmark::State& state) { - using FunctorType = Brdf; - - double material[] = {1., 2., 3., 4., 5., 6., 7., 8., 9., 10.}; - auto c = Eigen::Vector3d(0.1, 0.2, 0.3); - auto n = Eigen::Vector3d(-0.1, 0.5, 0.2).normalized(); - auto v = Eigen::Vector3d(0.5, -0.2, 0.9).normalized(); - auto l = Eigen::Vector3d(-0.3, 0.4, -0.3).normalized(); - auto x = Eigen::Vector3d(0.5, 0.7, -0.1).normalized(); - auto y = Eigen::Vector3d(0.2, -0.2, -0.2).normalized(); - - double* parameters[7] = { - material, c.data(), n.data(), v.data(), l.data(), x.data(), y.data()}; - - double jacobian[(10 + 6 * 3) * 3]; - double residuals[3]; - // clang-format off - double* jacobians[7] = { - jacobian + 0, jacobian + 10 * 3, jacobian + 13 * 3, - jacobian + 16 * 3, jacobian + 19 * 3, jacobian + 22 * 3, - jacobian + 25 * 3, - }; - // clang-format on - - std::unique_ptr<ceres::CostFunction> cost_function = CostFunctionFactory< - kIsDynamic>::template Create<FunctorType, 3, 10, 3, 3, 3, 3, 3, 3>(); - - for (auto _ : state) { - cost_function->Evaluate( - parameters, residuals, state.range(0) ? jacobians : nullptr); - } -} - -BENCHMARK_TEMPLATE(BM_BrdfAutoDiff, kNotDynamic)->Arg(0)->Arg(1); -BENCHMARK_TEMPLATE(BM_BrdfAutoDiff, kDynamic)->Arg(0)->Arg(1); - -} // namespace ceres - -BENCHMARK_MAIN();
diff --git a/internal/ceres/autodiff_benchmarks/brdf_cost_function.h b/internal/ceres/autodiff_benchmarks/brdf_cost_function.h deleted file mode 100644 index 41cfcfa..0000000 --- a/internal/ceres/autodiff_benchmarks/brdf_cost_function.h +++ /dev/null
@@ -1,223 +0,0 @@ -// 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_AUTODIFF_BENCHMARK_BRDF_COST_FUNCTION_H_ -#define CERES_INTERNAL_AUTODIFF_BENCHMARK_BRDF_COST_FUNCTION_H_ - -#include <Eigen/Core> -#include <cmath> - -#include "ceres/constants.h" - -namespace ceres { - -// The brdf is based on: -// Burley, Brent, and Walt Disney Animation Studios. "Physically-based shading -// at disney." ACM SIGGRAPH. Vol. 2012. 2012. -// -// The implementation is based on: -// https://github.com/wdas/brdf/blob/master/src/brdfs/disney.brdf -struct Brdf { - public: - template <typename T> - inline bool operator()(const T* const material, - const T* const c_ptr, - const T* const n_ptr, - const T* const v_ptr, - const T* const l_ptr, - const T* const x_ptr, - const T* const y_ptr, - T* residual) const { - using Vec3 = Eigen::Matrix<T, 3, 1>; - - T metallic = material[0]; - T subsurface = material[1]; - T specular = material[2]; - T roughness = material[3]; - T specular_tint = material[4]; - T anisotropic = material[5]; - T sheen = material[6]; - T sheen_tint = material[7]; - T clearcoat = material[8]; - T clearcoat_gloss = material[9]; - - Eigen::Map<const Vec3> c(c_ptr); - Eigen::Map<const Vec3> n(n_ptr); - Eigen::Map<const Vec3> v(v_ptr); - Eigen::Map<const Vec3> l(l_ptr); - Eigen::Map<const Vec3> x(x_ptr); - Eigen::Map<const Vec3> y(y_ptr); - - const T n_dot_l = n.dot(l); - const T n_dot_v = n.dot(v); - - const Vec3 l_p_v = l + v; - const Vec3 h = l_p_v / l_p_v.norm(); - - const T n_dot_h = n.dot(h); - const T l_dot_h = l.dot(h); - - const T h_dot_x = h.dot(x); - const T h_dot_y = h.dot(y); - - const T c_dlum = T(0.3) * c[0] + T(0.6) * c[1] + T(0.1) * c[2]; - - const Vec3 c_tint = c / c_dlum; - - const Vec3 c_spec0 = - Lerp(specular * T(0.08) * - Lerp(Vec3(T(1), T(1), T(1)), c_tint, specular_tint), - c, - metallic); - const Vec3 c_sheen = Lerp(Vec3(T(1), T(1), T(1)), c_tint, sheen_tint); - - // Diffuse fresnel - go from 1 at normal incidence to .5 at grazing - // and mix in diffuse retro-reflection based on roughness - const T fl = SchlickFresnel(n_dot_l); - const T fv = SchlickFresnel(n_dot_v); - const T fd_90 = T(0.5) + T(2) * l_dot_h * l_dot_h * roughness; - const T fd = Lerp(T(1), fd_90, fl) * Lerp(T(1), fd_90, fv); - - // Based on Hanrahan-Krueger brdf approximation of isotropic bssrdf - // 1.25 scale is used to (roughly) preserve albedo - // Fss90 used to "flatten" retroreflection based on roughness - const T fss_90 = l_dot_h * l_dot_h * roughness; - const T fss = Lerp(T(1), fss_90, fl) * Lerp(T(1), fss_90, fv); - const T ss = - T(1.25) * (fss * (T(1) / (n_dot_l + n_dot_v) - T(0.5)) + T(0.5)); - - // specular - const T eps = T(0.001); - const T aspct = Aspect(anisotropic); - const T ax_temp = Square(roughness) / aspct; - const T ay_temp = Square(roughness) * aspct; - const T ax = (ax_temp < eps ? eps : ax_temp); - const T ay = (ay_temp < eps ? eps : ay_temp); - const T ds = GTR2Aniso(n_dot_h, h_dot_x, h_dot_y, ax, ay); - const T fh = SchlickFresnel(l_dot_h); - const Vec3 fs = Lerp(c_spec0, Vec3(T(1), T(1), T(1)), fh); - const T roughg = Square(roughness * T(0.5) + T(0.5)); - const T ggxn_dot_l = SmithG_GGX(n_dot_l, roughg); - const T ggxn_dot_v = SmithG_GGX(n_dot_v, roughg); - const T gs = ggxn_dot_l * ggxn_dot_v; - - // sheen - const Vec3 f_sheen = fh * sheen * c_sheen; - - // clearcoat (ior = 1.5 -> F0 = 0.04) - const T a = Lerp(T(0.1), T(0.001), clearcoat_gloss); - const T dr = GTR1(n_dot_h, a); - const T fr = Lerp(T(0.04), T(1), fh); - const T cggxn_dot_l = SmithG_GGX(n_dot_l, T(0.25)); - const T cggxn_dot_v = SmithG_GGX(n_dot_v, T(0.25)); - const T gr = cggxn_dot_l * cggxn_dot_v; - - const Vec3 result_no_cosine = - (T(1.0 / constants::pi) * Lerp(fd, ss, subsurface) * c + f_sheen) * - (T(1) - metallic) + - gs * fs * ds + - Vec3(T(0.25), T(0.25), T(0.25)) * clearcoat * gr * fr * dr; - const Vec3 result = n_dot_l * result_no_cosine; - residual[0] = result(0); - residual[1] = result(1); - residual[2] = result(2); - - return true; - } - - template <typename T> - inline T SchlickFresnel(const T& u) const { - T m = T(1) - u; - const T m2 = m * m; - return m2 * m2 * m; // (1-u)^5 - } - - template <typename T> - inline T Aspect(const T& anisotropic) const { - return T(sqrt(T(1) - anisotropic * T(0.9))); - } - - template <typename T> - inline T SmithG_GGX(const T& n_dot_v, const T& alpha_g) const { - const T a = alpha_g * alpha_g; - const T b = n_dot_v * n_dot_v; - return T(1) / (n_dot_v + T(sqrt(a + b - a * b))); - } - - // Generalized-Trowbridge-Reitz (GTR) Microfacet Distribution - // See paper, Appendix B - template <typename T> - inline T GTR1(const T& n_dot_h, const T& a) const { - T result = T(0); - - if (a >= T(1)) { - result = T(1 / constants::pi); - } else { - const T a2 = a * a; - const T t = T(1) + (a2 - T(1)) * n_dot_h * n_dot_h; - result = (a2 - T(1)) / (T(constants::pi) * T(log(a2) * t)); - } - return result; - } - - template <typename T> - inline T GTR2Aniso(const T& n_dot_h, - const T& h_dot_x, - const T& h_dot_y, - const T& ax, - const T& ay) const { - return T(1) / (T(constants::pi) * ax * ay * - Square(Square(h_dot_x / ax) + Square(h_dot_y / ay) + - n_dot_h * n_dot_h)); - } - - template <typename T> - inline T Lerp(const T& a, const T& b, const T& u) const { - return a + u * (b - a); - } - - template <typename Derived1, typename Derived2> - inline typename Derived1::PlainObject Lerp( - const Eigen::MatrixBase<Derived1>& a, - const Eigen::MatrixBase<Derived2>& b, - typename Derived1::Scalar alpha) const { - return (typename Derived1::Scalar(1) - alpha) * a + alpha * b; - } - - template <typename T> - inline T Square(const T& x) const { - return x * x; - } -}; - -} // namespace ceres - -#endif // CERES_INTERNAL_AUTODIFF_BENCHMARK_BRDF_COST_FUNCTION_H_
diff --git a/internal/ceres/autodiff_benchmarks/constant_cost_function.h b/internal/ceres/autodiff_benchmarks/constant_cost_function.h deleted file mode 100644 index 2bbe4cb..0000000 --- a/internal/ceres/autodiff_benchmarks/constant_cost_function.h +++ /dev/null
@@ -1,66 +0,0 @@ -// 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_AUTODIFF_BENCHMARKS_CONSTANT_COST_FUNCTIONS_H_ -#define CERES_INTERNAL_AUTODIFF_BENCHMARKS_CONSTANT_COST_FUNCTIONS_H_ - -#include "ceres/sized_cost_function.h" - -namespace ceres { - -template <int kParameterBlockSize> -struct ConstantCostFunction { - template <typename T> - inline bool operator()(const T* const x, T* residuals) const { - residuals[0] = T(5); - return true; - } -}; - -template <int kParameterBlockSize> -struct AnalyticConstantCostFunction - : public ceres::SizedCostFunction<1, kParameterBlockSize> { - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - residuals[0] = 5.0; - if (jacobians) { - if (jacobians[0]) { - memset(jacobians[0], 0, sizeof(double) * kParameterBlockSize); - } - } - return true; - } -}; - -} // namespace ceres - -#endif // CERES_INTERNAL_AUTODIFF_BENCHMARKS_CONSTANT_COST_FUNCTIONS_H_
diff --git a/internal/ceres/autodiff_benchmarks/linear_cost_functions.h b/internal/ceres/autodiff_benchmarks/linear_cost_functions.h deleted file mode 100644 index 2f2552f..0000000 --- a/internal/ceres/autodiff_benchmarks/linear_cost_functions.h +++ /dev/null
@@ -1,58 +0,0 @@ -// 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_AUTODIFF_BENCHMARKS_LINEAR_COST_FUNCTIONS_H_ -#define CERES_INTERNAL_AUTODIFF_BENCHMARKS_LINEAR_COST_FUNCTIONS_H_ - -#include "ceres/rotation.h" - -namespace ceres { - -struct Linear1CostFunction { - template <typename T> - inline bool operator()(const T* const x, T* residuals) const { - residuals[0] = x[0] + T(10); - return true; - } -}; - -struct Linear10CostFunction { - template <typename T> - inline bool operator()(const T* const x, T* residuals) const { - for (int i = 0; i < 10; ++i) { - residuals[i] = x[i] + T(i); - } - return true; - } -}; -} // namespace ceres - -#endif // CERES_INTERNAL_AUTODIFF_BENCHMARKS_LINEAR_COST_FUNCTIONS_H_
diff --git a/internal/ceres/autodiff_benchmarks/photometric_error.h b/internal/ceres/autodiff_benchmarks/photometric_error.h deleted file mode 100644 index 6572267..0000000 --- a/internal/ceres/autodiff_benchmarks/photometric_error.h +++ /dev/null
@@ -1,190 +0,0 @@ -// 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: nikolaus@nikolaus-demmel.de (Nikolaus Demmel) -// -// -#ifndef CERES_INTERNAL_AUTODIFF_BENCHMARK_PHOTOMETRIC_ERROR_H_ -#define CERES_INTERNAL_AUTODIFF_BENCHMARK_PHOTOMETRIC_ERROR_H_ - -#include <Eigen/Dense> - -#include "ceres/cubic_interpolation.h" - -namespace ceres { - -// Photometric residual that computes the intensity difference for a patch -// between host and target frame. The point is parameterized with inverse -// distance relative to the host frame. The relative pose between host and -// target frame is computed from their respective absolute poses. -// -// The residual is similar to the one defined by Engel et al. [1]. Differences -// include: -// -// 1. Use of a camera model based on spherical projection, namely the enhanced -// unified camera model [2][3]. This is intended to bring some variability to -// the benchmark compared to the SnavelyReprojection that uses a -// polynomial-based distortion model. -// -// 2. To match the camera model, inverse distance parameterization is used for -// points instead of inverse depth [4]. -// -// 3. For simplicity, camera intrinsics are assumed constant, and thus host -// frame points are passed as (unprojected) bearing vectors, which avoids the -// need for an 'unproject' function. -// -// 4. Some details of the residual in [1] are omitted for simplicity: The -// brightness transform parameters [a,b], the constant pre-weight w, and the -// per-pixel robust norm. -// -// [1] J. Engel, V. Koltun and D. Cremers, "Direct Sparse Odometry," in IEEE -// Transactions on Pattern Analysis and Machine Intelligence, vol. 40, no. 3, -// pp. 611-625, 1 March 2018. -// -// [2] B. Khomutenko, G. Garcia and P. Martinet, "An Enhanced Unified Camera -// Model," in IEEE Robotics and Automation Letters, vol. 1, no. 1, pp. 137-144, -// Jan. 2016. -// -// [3] V. Usenko, N. Demmel and D. Cremers, "The Double Sphere Camera Model," -// 2018 International Conference on 3D Vision (3DV), Verona, 2018, pp. 552-560. -// -// [4] H. Matsuki, L. von Stumberg, V. Usenko, J. Stückler and D. Cremers, -// "Omnidirectional DSO: Direct Sparse Odometry With Fisheye Cameras," in IEEE -// Robotics and Automation Letters, vol. 3, no. 4, pp. 3693-3700, Oct. 2018. -template <int PATCH_SIZE_ = 8> -struct PhotometricError { - static constexpr int PATCH_SIZE = PATCH_SIZE_; - static constexpr int POSE_SIZE = 7; - static constexpr int POINT_SIZE = 1; - - using Grid = Grid2D<uint8_t, 1>; - using Interpolator = BiCubicInterpolator<Grid>; - using Intrinsics = Eigen::Array<double, 6, 1>; - - template <typename T> - using Patch = Eigen::Array<T, PATCH_SIZE, 1>; - - template <typename T> - using PatchVectors = Eigen::Matrix<T, 3, PATCH_SIZE>; - - PhotometricError(const Patch<double>& intensities_host, - const PatchVectors<double>& bearings_host, - const Interpolator& image_target, - const Intrinsics& intrinsics) - : intensities_host_(intensities_host), - bearings_host_(bearings_host), - image_target_(image_target), - intrinsics_(intrinsics) {} - - template <typename T> - inline bool Project(Eigen::Matrix<T, 2, 1>& proj, - const Eigen::Matrix<T, 3, 1>& p) const { - const double& fx = intrinsics_[0]; - const double& fy = intrinsics_[1]; - const double& cx = intrinsics_[2]; - const double& cy = intrinsics_[3]; - const double& alpha = intrinsics_[4]; - const double& beta = intrinsics_[5]; - - const T rho2 = beta * (p.x() * p.x() + p.y() * p.y()) + p.z() * p.z(); - const T rho = sqrt(rho2); - - // Check if 3D point is in domain of projection function. - // See (8) and (17) in [3]. - constexpr double NUMERIC_EPSILON = 1e-10; - const double w = - alpha > 0.5 ? (1.0 - alpha) / alpha : alpha / (1.0 - alpha); - if (p.z() <= -w * rho + NUMERIC_EPSILON) { - return false; - } - - const T norm = alpha * rho + (1.0 - alpha) * p.z(); - const T norm_inv = 1.0 / norm; - - const T mx = p.x() * norm_inv; - const T my = p.y() * norm_inv; - - proj[0] = fx * mx + cx; - proj[1] = fy * my + cy; - - return true; - } - - template <typename T> - inline bool operator()(const T* const pose_host_ptr, - const T* const pose_target_ptr, - const T* const idist_ptr, - T* residuals_ptr) const { - Eigen::Map<const Eigen::Quaternion<T>> q_w_h(pose_host_ptr); - Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_h(pose_host_ptr + 4); - Eigen::Map<const Eigen::Quaternion<T>> q_w_t(pose_target_ptr); - Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_t(pose_target_ptr + 4); - const T& idist = *idist_ptr; - Eigen::Map<Patch<T>> residuals(residuals_ptr); - - // Compute relative pose from host to target frame. - const Eigen::Quaternion<T> q_t_h = q_w_t.conjugate() * q_w_h; - const Eigen::Matrix<T, 3, 3> R_t_h = q_t_h.toRotationMatrix(); - const Eigen::Matrix<T, 3, 1> t_t_h = q_w_t.conjugate() * (t_w_h - t_w_t); - - // Transform points from host to target frame. 3D point in target frame is - // scaled by idist for numerical stability when idist is close to 0 - // (projection is invariant to scaling). - PatchVectors<T> p_target_scaled = - (R_t_h * bearings_host_).colwise() + idist * t_t_h; - - // Project points and interpolate image. - Patch<T> intensities_target; - for (int i = 0; i < p_target_scaled.cols(); ++i) { - Eigen::Matrix<T, 2, 1> uv; - if (!Project(uv, Eigen::Matrix<T, 3, 1>(p_target_scaled.col(i)))) { - // If any point of the patch is outside the domain of the projection - // function, the residual cannot be evaluated. For the benchmark we want - // to avoid this case and thus return false; - return false; - } - - // Mind the order of u and v: Evaluate takes (row, column), but u is - // left-to-right and v top-to-bottom image axis. - image_target_.Evaluate(uv[1], uv[0], &intensities_target[i]); - } - - // Residual is intensity difference between host and target frame. - residuals = intensities_target - intensities_host_; - - return true; - } - - private: - const Patch<double>& intensities_host_; - const PatchVectors<double>& bearings_host_; - const Interpolator& image_target_; - const Intrinsics& intrinsics_; -}; -} // namespace ceres -#endif // CERES_INTERNAL_AUTODIFF_BENCHMARK_PHOTOMETRIC_ERROR_H_
diff --git a/internal/ceres/autodiff_benchmarks/relative_pose_error.h b/internal/ceres/autodiff_benchmarks/relative_pose_error.h deleted file mode 100644 index a54a92f..0000000 --- a/internal/ceres/autodiff_benchmarks/relative_pose_error.h +++ /dev/null
@@ -1,87 +0,0 @@ -// 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: nikolaus@nikolaus-demmel.de (Nikolaus Demmel) -// -// -#ifndef CERES_INTERNAL_AUTODIFF_BENCHMARK_RELATIVE_POSE_ERROR_H_ -#define CERES_INTERNAL_AUTODIFF_BENCHMARK_RELATIVE_POSE_ERROR_H_ - -#include <Eigen/Dense> -#include <utility> - -#include "ceres/rotation.h" - -namespace ceres { - -// Relative pose error as one might use in SE(3) pose graph optimization. -// The measurement is a relative pose T_i_j, and the parameters are absolute -// poses T_w_i and T_w_j. For the residual we use the log of the the residual -// pose, in split representation SO(3) x R^3. -struct RelativePoseError { - RelativePoseError(Eigen::Quaterniond q_i_j, Eigen::Vector3d t_i_j) - : meas_q_i_j_(std::move(q_i_j)), meas_t_i_j_(std::move(t_i_j)) {} - - template <typename T> - inline bool operator()(const T* const pose_i_ptr, - const T* const pose_j_ptr, - T* residuals_ptr) const { - Eigen::Map<const Eigen::Quaternion<T>> q_w_i(pose_i_ptr); - Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_i(pose_i_ptr + 4); - Eigen::Map<const Eigen::Quaternion<T>> q_w_j(pose_j_ptr); - Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_j(pose_j_ptr + 4); - Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr); - - // Compute estimate of relative pose from i to j. - const Eigen::Quaternion<T> est_q_j_i = q_w_j.conjugate() * q_w_i; - const Eigen::Matrix<T, 3, 1> est_t_j_i = - q_w_j.conjugate() * (t_w_i - t_w_j); - - // Compute residual pose. - const Eigen::Quaternion<T> res_q = meas_q_i_j_.cast<T>() * est_q_j_i; - const Eigen::Matrix<T, 3, 1> res_t = - meas_q_i_j_.cast<T>() * est_t_j_i + meas_t_i_j_; - - // Convert quaternion to ceres convention (Eigen stores xyzw, Ceres wxyz). - Eigen::Matrix<T, 4, 1> res_q_ceres; - res_q_ceres << res_q.w(), res_q.vec(); - - // Residual is log of pose. Use split representation SO(3) x R^3. - QuaternionToAngleAxis(res_q_ceres.data(), residuals.data()); - residuals.template bottomRows<3>() = res_t; - - return true; - } - - private: - // Measurement of relative pose from j to i. - Eigen::Quaterniond meas_q_i_j_; - Eigen::Vector3d meas_t_i_j_; -}; -} // namespace ceres -#endif // CERES_INTERNAL_AUTODIFF_BENCHMARK_RELATIVE_POSE_ERROR_H_
diff --git a/internal/ceres/autodiff_benchmarks/snavely_reprojection_error.h b/internal/ceres/autodiff_benchmarks/snavely_reprojection_error.h deleted file mode 100644 index 439ee9b..0000000 --- a/internal/ceres/autodiff_benchmarks/snavely_reprojection_error.h +++ /dev/null
@@ -1,86 +0,0 @@ -// 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_AUTODIFF_BENCHMARK_SNAVELY_REPROJECTION_ERROR_H_ -#define CERES_INTERNAL_AUTODIFF_BENCHMARK_SNAVELY_REPROJECTION_ERROR_H_ - -#include "ceres/rotation.h" - -namespace ceres { - -struct SnavelyReprojectionError { - SnavelyReprojectionError(double observed_x, double observed_y) - : observed_x(observed_x), observed_y(observed_y) {} - - template <typename T> - inline bool operator()(const T* const camera, - const T* const point, - T* residuals) const { - T ox = T(observed_x); - T oy = 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; - } - double observed_x; - double observed_y; -}; -} // namespace ceres -#endif // CERES_INTERNAL_AUTODIFF_BENCHMARK_SNAVELY_REPROJECTION_ERROR_H_
diff --git a/internal/ceres/benchmark_cost_functions.h b/internal/ceres/benchmark_cost_functions.h new file mode 100644 index 0000000..464ab14 --- /dev/null +++ b/internal/ceres/benchmark_cost_functions.h
@@ -0,0 +1,1407 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2026 Google Inc. All rights reserved. +// Copyright (c) 2016, ETH Zurich and UNC Chapel Hill. +// Copyright (c) 2016-2026, The COLMAP Contributors. +// Copyright (c) 2011-2013, libmv authors. +// 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., ETH Zurich, UNC Chapel Hill, 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. +// +// SPDX-License-Identifier: BSD-3-Clause AND MIT +// +// Authors: darius.rueckert@fau.de (Darius Rueckert) +// nikolaus@nikolaus-demmel.de (Nikolaus Demmel) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BENCHMARK_COST_FUNCTIONS_H_ +#define CERES_INTERNAL_BENCHMARK_COST_FUNCTIONS_H_ + +#include <algorithm> +#include <array> +#include <cmath> +#include <cstdint> +#include <memory> +#include <numeric> +#include <random> +#include <tuple> +#include <type_traits> +#include <utility> +#include <vector> + +#include "Eigen/Core" +#include "Eigen/Dense" +#include "Eigen/Geometry" +#include "ceres/ceres.h" +#include "ceres/constants.h" +#include "ceres/cubic_interpolation.h" +#include "ceres/rotation.h" + +namespace ceres::internal { + +// Owns fixed-size parameter block arrays for a benchmark cost functor and +// exposes an array of pointers suitable for CostFunction::Evaluate. +template <int... Ns> +class ParameterBlockStorage { + public: + static constexpr int kNumBlocks = sizeof...(Ns); + + explicit ParameterBlockStorage(const std::array<double, Ns>&... blocks) + : blocks_(blocks...) {} + + std::array<const double*, kNumBlocks> Pointers() const { + return PointersImpl(std::make_index_sequence<kNumBlocks>()); + } + + private: + template <size_t... Indices> + std::array<const double*, kNumBlocks> PointersImpl( + std::index_sequence<Indices...>) const { + return {std::get<Indices>(blocks_).data()...}; + } + + std::tuple<std::array<double, Ns>...> blocks_; +}; + +// Adapts a fixed-arity cost functor `CostFunctor(p0, p1, ..., residuals)` to +// the dynamic-arity signature `operator()(const T* const* parameters, T* +// residuals)` expected by DynamicAutoDiffCostFunction and +// DynamicNumericDiffCostFunction. +template <typename CostFunctor, int kNumParameterBlocks> +class DynamicCostFunctorAdapter { + public: + template <typename... Args> + explicit DynamicCostFunctorAdapter(Args&&... args) + : functor_(std::forward<Args>(args)...) {} + + template <typename T> + EIGEN_STRONG_INLINE bool operator()(const T* const* parameters, + T* residuals) const { + return Apply( + parameters, residuals, std::make_index_sequence<kNumParameterBlocks>()); + } + + private: + template <typename T, size_t... Indices> + EIGEN_STRONG_INLINE bool Apply(const T* const* parameters, + T* residuals, + std::index_sequence<Indices...>) const { + return functor_(parameters[Indices]..., residuals); + } + + CostFunctor functor_; +}; + +// Base class declaring the residual count and parameter block dimensions of a +// benchmark cost functor, along with CostFunction, Manifold, and LossFunction +// factories. +template <int kResiduals, int... Ns> +struct BenchmarkCostFunctor { + static constexpr int kNumResiduals = kResiduals; + static constexpr int kNumParameterBlocks = sizeof...(Ns); + static constexpr int kTotalParameters = (Ns + ...); + static constexpr std::array<int, kNumParameterBlocks> kBlockSizes = {Ns...}; + using ParameterBlocks = ParameterBlockStorage<Ns...>; + + template <typename CostFunctor, typename... Args> + static std::unique_ptr<CostFunction> CreateAutoDiff(Args&&... args) { + return std::make_unique< + AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns...>>( + std::make_unique<CostFunctor>(std::forward<Args>(args)...)); + } + + template <typename CostFunctor, typename... Args> + static std::unique_ptr<CostFunction> CreateAutoDiffDynamicResiduals( + Args&&... args) { + return std::make_unique<AutoDiffCostFunction<CostFunctor, DYNAMIC, Ns...>>( + std::make_unique<CostFunctor>(std::forward<Args>(args)...), + kNumResiduals); + } + + template <typename CostFunctor, typename... Args> + static std::unique_ptr<CostFunction> CreateDynamicAutoDiff(Args&&... args) { + using Adapter = DynamicCostFunctorAdapter<CostFunctor, kNumParameterBlocks>; + auto cost_function = std::make_unique<DynamicAutoDiffCostFunction<Adapter>>( + std::make_unique<Adapter>(std::forward<Args>(args)...)); + (cost_function->AddParameterBlock(Ns), ...); + cost_function->SetNumResiduals(kNumResiduals); + return cost_function; + } + + template <typename CostFunctor, + NumericDiffMethodType kMethod, + typename... Args> + static std::unique_ptr<CostFunction> CreateNumericDiff(Args&&... args) { + return std::make_unique< + NumericDiffCostFunction<CostFunctor, kMethod, kNumResiduals, Ns...>>( + std::make_unique<CostFunctor>(std::forward<Args>(args)...)); + } + + template <typename CostFunctor, + NumericDiffMethodType kMethod, + typename... Args> + static std::unique_ptr<CostFunction> CreateDynamicNumericDiff( + Args&&... args) { + using Adapter = DynamicCostFunctorAdapter<CostFunctor, kNumParameterBlocks>; + auto cost_function = + std::make_unique<DynamicNumericDiffCostFunction<Adapter, kMethod>>( + std::make_unique<const Adapter>(std::forward<Args>(args)...)); + (cost_function->AddParameterBlock(Ns), ...); + cost_function->SetNumResiduals(kNumResiduals); + return cost_function; + } + + static std::unique_ptr<Manifold> MakeManifold(int /*block_index*/) { + return nullptr; + } + + static std::unique_ptr<LossFunction> MakeLossFunction() { return nullptr; } +}; + +inline std::array<double, 3> NormalizedVector3(double x, double y, double z) { + std::array v{x, y, z}; + Eigen::Map<Eigen::Vector3d>(v.data()).normalize(); + return v; +} + +inline std::array<double, 4> NormalizedQuaternion(double qx, + double qy, + double qz, + double qw) { + std::array q{qx, qy, qz, qw}; + Eigen::Map<Eigen::Quaterniond>(q.data()).normalize(); + return q; +} + +inline std::array<double, 7> NormalizedPose(double qx, + double qy, + double qz, + double qw, + double tx, + double ty, + double tz) { + std::array pose{qx, qy, qz, qw, tx, ty, tz}; + Eigen::Map<Eigen::Quaterniond>(pose.data()).normalize(); + return pose; +} + +// Converts the angle-axis rotation and translation prefix of a 9-parameter BAL +// camera `[rx, ry, rz, tx, ty, tz, ...]` into a 7-parameter pose `[qx, qy, qz, +// qw, tx, ty, tz]` with an Eigen-convention quaternion. +inline std::array<double, 7> PoseFromBALCamera(const double* bal_camera) { + double q_wxyz[4]; + AngleAxisToQuaternion(bal_camera, q_wxyz); + return {q_wxyz[1], + q_wxyz[2], + q_wxyz[3], + q_wxyz[0], + bal_camera[3], + bal_camera[4], + bal_camera[5]}; +} + +template <int kParameterBlockSize> +struct ConstantCostFunction : BenchmarkCostFunctor<1, kParameterBlockSize> { + using + typename BenchmarkCostFunctor<1, kParameterBlockSize>::ParameterBlocks; + + static ParameterBlocks MakeParameters() { + std::array<double, kParameterBlockSize> x{}; + std::iota(x.begin(), x.end(), 0.0); + return ParameterBlocks(x); + } + + template <typename T> + inline bool operator()(const T* const x, T* residuals) const { + residuals[0] = T(5); + return true; + } +}; + +template <int kDimension> +struct LinearCostFunction : BenchmarkCostFunctor<kDimension, kDimension> { + using typename BenchmarkCostFunctor<kDimension, kDimension>::ParameterBlocks; + + static ParameterBlocks MakeParameters() { + std::array<double, kDimension> x{}; + std::iota(x.begin(), x.end(), 1.0); + return ParameterBlocks(x); + } + + template <typename T> + inline bool operator()(const T* const x, T* residuals) const { + for (int i = 0; i < kDimension; ++i) { + residuals[i] = x[i] + T(i); + } + return true; + } +}; + +// From the NIST problem collection. +struct Rat43CostFunctor : BenchmarkCostFunctor<1, 4> { + explicit Rat43CostFunctor(const double x = 0.2, const double y = 0.3) + : x_(x), y_(y) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks({1., 2., 3., 4.}); + } + + template <typename T> + inline bool operator()(const T* parameters, T* residuals) const { + const T& b1 = parameters[0]; + const T& b2 = parameters[1]; + const T& b3 = parameters[2]; + const T& b4 = parameters[3]; + residuals[0] = b1 * pow(1.0 + exp(b2 - b3 * x_), -1.0 / b4) - y_; + return true; + } + + private: + const double x_; + const double y_; +}; + +struct SnavelyReprojectionError : BenchmarkCostFunctor<2, 9, 3> { + static constexpr int kPointBlockIndex = 1; + + explicit SnavelyReprojectionError(double observed_x = 0.2, + double observed_y = 0.3) + : observed_x(observed_x), observed_y(observed_y) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks({1., 2., 3., 4., 5., 6., 7., 8., 9.}, {1., 2., 3.}); + } + + static ParameterBlocks MakeParametersFromBAL(const double* bal_camera, + const double* bal_point) { + std::array<double, 9> camera{}; + std::array<double, 3> point{}; + std::copy_n(bal_camera, 9, camera.data()); + std::copy_n(bal_point, 3, point.data()); + return ParameterBlocks(camera, point); + } + + template <typename T> + inline bool operator()(const T* const camera, + const T* const point, + T* residuals) const { + T ox = T(observed_x); + T oy = 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; + } + double observed_x; + double observed_y; +}; + +// Photometric residual that computes the intensity difference for a patch +// between host and target frame. The point is parameterized with inverse +// distance relative to the host frame. The relative pose between host and +// target frame is computed from their respective absolute poses. +// +// The residual is similar to the one defined by Engel et al. [1]. Differences +// include: +// +// 1. Use of a camera model based on spherical projection, namely the enhanced +// unified camera model [2][3]. This is intended to bring some variability to +// the benchmark compared to the SnavelyReprojection that uses a +// polynomial-based distortion model. +// +// 2. To match the camera model, inverse distance parameterization is used for +// points instead of inverse depth [4]. +// +// 3. For simplicity, camera intrinsics are assumed constant, and thus host +// frame points are passed as (unprojected) bearing vectors, which avoids the +// need for an 'unproject' function. +// +// 4. Some details of the residual in [1] are omitted for simplicity: The +// brightness transform parameters [a,b], the constant pre-weight w, and the +// per-pixel robust norm. +// +// [1] J. Engel, V. Koltun and D. Cremers, "Direct Sparse Odometry," in IEEE +// Transactions on Pattern Analysis and Machine Intelligence, vol. 40, no. 3, +// pp. 611-625, 1 March 2018. +// +// [2] B. Khomutenko, G. Garcia and P. Martinet, "An Enhanced Unified Camera +// Model," in IEEE Robotics and Automation Letters, vol. 1, no. 1, pp. 137-144, +// Jan. 2016. +// +// [3] V. Usenko, N. Demmel and D. Cremers, "The Double Sphere Camera Model," +// 2018 International Conference on 3D Vision (3DV), Verona, 2018, pp. 552-560. +// +// [4] H. Matsuki, L. von Stumberg, V. Usenko, J. Stückler and D. Cremers, +// "Omnidirectional DSO: Direct Sparse Odometry With Fisheye Cameras," in IEEE +// Robotics and Automation Letters, vol. 3, no. 4, pp. 3693-3700, Oct. 2018. +template <int PATCH_SIZE_ = 8> +struct PhotometricError : BenchmarkCostFunctor<PATCH_SIZE_, 7, 7, 1> { + static constexpr int PATCH_SIZE = PATCH_SIZE_; + static constexpr int POSE_SIZE = 7; + static constexpr int POINT_SIZE = 1; + using typename BenchmarkCostFunctor<PATCH_SIZE_, 7, 7, 1>::ParameterBlocks; + + using ImageType = Eigen::Matrix<uint8_t, 128, 128, Eigen::RowMajor>; + using Grid = Grid2D<uint8_t, 1>; + using Interpolator = BiCubicInterpolator<Grid>; + using Intrinsics = Eigen::Array<double, 6, 1>; + + template <typename T> + using Patch = Eigen::Array<T, PATCH_SIZE, 1>; + + template <typename T> + using PatchVectors = Eigen::Matrix<T, 3, PATCH_SIZE>; + + PhotometricError() + : grid_(image_.data(), 0, image_.rows(), 0, image_.cols()), + image_target_(grid_) { + std::mt19937 gen(42); + std::uniform_real_distribution<double> uniform01(0.0, 1.0); + std::uniform_int_distribution<unsigned int> uniform0255(0, 255); + + intensities_host_ = + Patch<double>::NullaryExpr([&]() { return uniform0255(gen); }); + bearings_host_ = + PatchVectors<double>::NullaryExpr([&]() { return uniform01(gen); }); + bearings_host_.row(2).array() = 1.0; + bearings_host_.colwise().normalize(); + + image_ = ImageType::NullaryExpr( + [&]() { return static_cast<uint8_t>(uniform0255(gen)); }); + intrinsics_ << 128, 128, 1, -1, 0.5, 0.5; + } + + PhotometricError(const PhotometricError&) = delete; + PhotometricError& operator=(const PhotometricError&) = delete; + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + NormalizedPose(1., 2., 3., 4., 5., 6., 7.), + NormalizedPose(1.1, 2.1, 3.1, 4.1, 5.1, 6.1, 7.1), + {1.0}); + } + + template <typename T> + inline bool Project(Eigen::Matrix<T, 2, 1>& proj, + const Eigen::Matrix<T, 3, 1>& p) const { + const double& fx = intrinsics_[0]; + const double& fy = intrinsics_[1]; + const double& cx = intrinsics_[2]; + const double& cy = intrinsics_[3]; + const double& alpha = intrinsics_[4]; + const double& beta = intrinsics_[5]; + + const T rho2 = beta * (p.x() * p.x() + p.y() * p.y()) + p.z() * p.z(); + const T rho = sqrt(rho2); + + // Check if 3D point is in domain of projection function. + // See (8) and (17) in [3]. + constexpr double NUMERIC_EPSILON = 1e-10; + const double w = + alpha > 0.5 ? (1.0 - alpha) / alpha : alpha / (1.0 - alpha); + if (p.z() <= -w * rho + NUMERIC_EPSILON) { + return false; + } + + const T norm = alpha * rho + (1.0 - alpha) * p.z(); + const T norm_inv = 1.0 / norm; + + const T mx = p.x() * norm_inv; + const T my = p.y() * norm_inv; + + proj[0] = fx * mx + cx; + proj[1] = fy * my + cy; + + return true; + } + + template <typename T> + inline bool operator()(const T* const pose_host_ptr, + const T* const pose_target_ptr, + const T* const idist_ptr, + T* residuals_ptr) const { + Eigen::Map<const Eigen::Quaternion<T>> q_w_h(pose_host_ptr); + Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_h(pose_host_ptr + 4); + Eigen::Map<const Eigen::Quaternion<T>> q_w_t(pose_target_ptr); + Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_t(pose_target_ptr + 4); + const T& idist = *idist_ptr; + Eigen::Map<Patch<T>> residuals(residuals_ptr); + + // Compute relative pose from host to target frame. + const Eigen::Quaternion<T> q_t_h = q_w_t.conjugate() * q_w_h; + const Eigen::Matrix<T, 3, 3> R_t_h = q_t_h.toRotationMatrix(); + const Eigen::Matrix<T, 3, 1> t_t_h = q_w_t.conjugate() * (t_w_h - t_w_t); + + // Transform points from host to target frame. 3D point in target frame is + // scaled by idist for numerical stability when idist is close to 0 + // (projection is invariant to scaling). + PatchVectors<T> p_target_scaled = + (R_t_h * bearings_host_).colwise() + idist * t_t_h; + + // Project points and interpolate image. + Patch<T> intensities_target; + for (int i = 0; i < p_target_scaled.cols(); ++i) { + Eigen::Matrix<T, 2, 1> uv; + if (!Project(uv, Eigen::Matrix<T, 3, 1>(p_target_scaled.col(i)))) { + // If any point of the patch is outside the domain of the projection + // function, the residual cannot be evaluated. For the benchmark we want + // to avoid this case and thus return false; + return false; + } + + // Mind the order of u and v: Evaluate takes (row, column), but u is + // left-to-right and v top-to-bottom image axis. + image_target_.Evaluate(uv[1], uv[0], &intensities_target[i]); + } + + // Residual is intensity difference between host and target frame. + residuals = intensities_target - intensities_host_; + + return true; + } + + private: + ImageType image_; + Grid grid_; + Interpolator image_target_; + Patch<double> intensities_host_; + PatchVectors<double> bearings_host_; + Intrinsics intrinsics_; +}; + +// Relative pose error as one might use in SE(3) pose graph optimization. +// The measurement is a relative pose T_i_j, and the parameters are absolute +// poses T_w_i and T_w_j. For the residual we use the log of the residual +// pose, in split representation SO(3) x R^3. +struct RelativePoseError : BenchmarkCostFunctor<6, 7, 7> { + explicit RelativePoseError( + Eigen::Quaterniond q_i_j = Eigen::Quaterniond(1, 2, 3, 4).normalized(), + Eigen::Vector3d t_i_j = Eigen::Vector3d(1, 2, 3)) + : meas_q_i_j_(std::move(q_i_j)), meas_t_i_j_(std::move(t_i_j)) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + NormalizedPose(1., 2., 3., 4., 5., 6., 7.), + NormalizedPose(1.1, 2.1, 3.1, 4.1, 5.1, 6.1, 7.1)); + } + + template <typename T> + inline bool operator()(const T* const pose_i_ptr, + const T* const pose_j_ptr, + T* residuals_ptr) const { + Eigen::Map<const Eigen::Quaternion<T>> q_w_i(pose_i_ptr); + Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_i(pose_i_ptr + 4); + Eigen::Map<const Eigen::Quaternion<T>> q_w_j(pose_j_ptr); + Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_w_j(pose_j_ptr + 4); + Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr); + + // Compute estimate of relative pose from i to j. + const Eigen::Quaternion<T> est_q_j_i = q_w_j.conjugate() * q_w_i; + const Eigen::Matrix<T, 3, 1> est_t_j_i = + q_w_j.conjugate() * (t_w_i - t_w_j); + + // Compute residual pose. + const Eigen::Quaternion<T> res_q = meas_q_i_j_.cast<T>() * est_q_j_i; + const Eigen::Matrix<T, 3, 1> res_t = + meas_q_i_j_.cast<T>() * est_t_j_i + meas_t_i_j_; + + // Convert quaternion to ceres convention (Eigen stores xyzw, Ceres wxyz). + Eigen::Matrix<T, 4, 1> res_q_ceres; + res_q_ceres << res_q.w(), res_q.vec(); + + // Residual is log of pose. Use split representation SO(3) x R^3. + QuaternionToAngleAxis(res_q_ceres.data(), residuals.data()); + residuals.template bottomRows<3>() = res_t; + + return true; + } + + private: + // Measurement of relative pose from j to i. + Eigen::Quaterniond meas_q_i_j_; + Eigen::Vector3d meas_t_i_j_; +}; + +// The brdf is based on: +// Burley, Brent, and Walt Disney Animation Studios. "Physically-based shading +// at disney." ACM SIGGRAPH. Vol. 2012. 2012. +// +// The implementation is based on: +// https://github.com/wdas/brdf/blob/master/src/brdfs/disney.brdf +struct Brdf : BenchmarkCostFunctor<3, 10, 3, 3, 3, 3, 3, 3> { + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {1., 2., 3., 4., 5., 6., 7., 8., 9., 10.}, + {0.1, 0.2, 0.3}, + NormalizedVector3(-0.1, 0.5, 0.2), + NormalizedVector3(0.5, -0.2, 0.9), + NormalizedVector3(-0.3, 0.4, -0.3), + NormalizedVector3(0.5, 0.7, -0.1), + NormalizedVector3(0.2, -0.2, -0.2)); + } + + template <typename T> + inline bool operator()(const T* const material, + const T* const c_ptr, + const T* const n_ptr, + const T* const v_ptr, + const T* const l_ptr, + const T* const x_ptr, + const T* const y_ptr, + T* residual) const { + using Vec3 = Eigen::Matrix<T, 3, 1>; + + T metallic = material[0]; + T subsurface = material[1]; + T specular = material[2]; + T roughness = material[3]; + T specular_tint = material[4]; + T anisotropic = material[5]; + T sheen = material[6]; + T sheen_tint = material[7]; + T clearcoat = material[8]; + T clearcoat_gloss = material[9]; + + Eigen::Map<const Vec3> c(c_ptr); + Eigen::Map<const Vec3> n(n_ptr); + Eigen::Map<const Vec3> v(v_ptr); + Eigen::Map<const Vec3> l(l_ptr); + Eigen::Map<const Vec3> x(x_ptr); + Eigen::Map<const Vec3> y(y_ptr); + + const T n_dot_l = n.dot(l); + const T n_dot_v = n.dot(v); + + const Vec3 l_p_v = l + v; + const Vec3 h = l_p_v / l_p_v.norm(); + + const T n_dot_h = n.dot(h); + const T l_dot_h = l.dot(h); + + const T h_dot_x = h.dot(x); + const T h_dot_y = h.dot(y); + + const T c_dlum = T(0.3) * c[0] + T(0.6) * c[1] + T(0.1) * c[2]; + + const Vec3 c_tint = c / c_dlum; + + const Vec3 c_spec0 = + Lerp(specular * T(0.08) * + Lerp(Vec3(T(1), T(1), T(1)), c_tint, specular_tint), + c, + metallic); + const Vec3 c_sheen = Lerp(Vec3(T(1), T(1), T(1)), c_tint, sheen_tint); + + // Diffuse fresnel - go from 1 at normal incidence to .5 at grazing + // and mix in diffuse retro-reflection based on roughness + const T fl = SchlickFresnel(n_dot_l); + const T fv = SchlickFresnel(n_dot_v); + const T fd_90 = T(0.5) + T(2) * l_dot_h * l_dot_h * roughness; + const T fd = Lerp(T(1), fd_90, fl) * Lerp(T(1), fd_90, fv); + + // Based on Hanrahan-Krueger brdf approximation of isotropic bssrdf + // 1.25 scale is used to (roughly) preserve albedo + // Fss90 used to "flatten" retroreflection based on roughness + const T fss_90 = l_dot_h * l_dot_h * roughness; + const T fss = Lerp(T(1), fss_90, fl) * Lerp(T(1), fss_90, fv); + const T ss = + T(1.25) * (fss * (T(1) / (n_dot_l + n_dot_v) - T(0.5)) + T(0.5)); + + // specular + const T eps = T(0.001); + const T aspct = Aspect(anisotropic); + const T ax_temp = Square(roughness) / aspct; + const T ay_temp = Square(roughness) * aspct; + const T ax = (ax_temp < eps ? eps : ax_temp); + const T ay = (ay_temp < eps ? eps : ay_temp); + const T ds = GTR2Aniso(n_dot_h, h_dot_x, h_dot_y, ax, ay); + const T fh = SchlickFresnel(l_dot_h); + const Vec3 fs = Lerp(c_spec0, Vec3(T(1), T(1), T(1)), fh); + const T roughg = Square(roughness * T(0.5) + T(0.5)); + const T ggxn_dot_l = SmithG_GGX(n_dot_l, roughg); + const T ggxn_dot_v = SmithG_GGX(n_dot_v, roughg); + const T gs = ggxn_dot_l * ggxn_dot_v; + + // sheen + const Vec3 f_sheen = fh * sheen * c_sheen; + + // clearcoat (ior = 1.5 -> F0 = 0.04) + const T a = Lerp(T(0.1), T(0.001), clearcoat_gloss); + const T dr = GTR1(n_dot_h, a); + const T fr = Lerp(T(0.04), T(1), fh); + const T cggxn_dot_l = SmithG_GGX(n_dot_l, T(0.25)); + const T cggxn_dot_v = SmithG_GGX(n_dot_v, T(0.25)); + const T gr = cggxn_dot_l * cggxn_dot_v; + + const Vec3 result_no_cosine = + (T(1.0 / constants::pi) * Lerp(fd, ss, subsurface) * c + f_sheen) * + (T(1) - metallic) + + gs * fs * ds + + Vec3(T(0.25), T(0.25), T(0.25)) * clearcoat * gr * fr * dr; + const Vec3 result = n_dot_l * result_no_cosine; + residual[0] = result(0); + residual[1] = result(1); + residual[2] = result(2); + + return true; + } + + template <typename T> + inline T SchlickFresnel(const T& u) const { + T m = T(1) - u; + const T m2 = m * m; + return m2 * m2 * m; // (1-u)^5 + } + + template <typename T> + inline T Aspect(const T& anisotropic) const { + return T(sqrt(T(1) - anisotropic * T(0.9))); + } + + template <typename T> + inline T SmithG_GGX(const T& n_dot_v, const T& alpha_g) const { + const T a = alpha_g * alpha_g; + const T b = n_dot_v * n_dot_v; + return T(1) / (n_dot_v + T(sqrt(a + b - a * b))); + } + + // Generalized-Trowbridge-Reitz (GTR) Microfacet Distribution + // See paper, Appendix B + template <typename T> + inline T GTR1(const T& n_dot_h, const T& a) const { + T result = T(0); + + if (a >= T(1)) { + result = T(1 / constants::pi); + } else { + const T a2 = a * a; + const T t = T(1) + (a2 - T(1)) * n_dot_h * n_dot_h; + result = (a2 - T(1)) / (T(constants::pi) * T(log(a2) * t)); + } + return result; + } + + template <typename T> + inline T GTR2Aniso(const T& n_dot_h, + const T& h_dot_x, + const T& h_dot_y, + const T& ax, + const T& ay) const { + return T(1) / (T(constants::pi) * ax * ay * + Square(Square(h_dot_x / ax) + Square(h_dot_y / ay) + + n_dot_h * n_dot_h)); + } + + template <typename T> + inline T Lerp(const T& a, const T& b, const T& u) const { + return a + u * (b - a); + } + + template <typename Derived1, typename Derived2> + inline typename Derived1::PlainObject Lerp( + const Eigen::MatrixBase<Derived1>& a, + const Eigen::MatrixBase<Derived2>& b, + typename Derived1::Scalar alpha) const { + return (typename Derived1::Scalar(1) - alpha) * a + alpha * b; + } + + template <typename T> + inline T Square(const T& x) const { + return x * x; + } +}; + +// Functors for benchmarking CostFunctionToFunctor (static sizes) and +// DynamicCostFunctionToFunctor (dynamic sizes). +// +// CostFunctionToFunctor and DynamicCostFunctionToFunctor allow an existing +// CostFunction object (which only accepts `double` inputs and computes `double` +// residuals and Jacobians via CostFunction::Evaluate) to be used as a templated +// functor inside another cost functor. When called with `Jet<double, N>` +// arguments during automatic differentiation of the outer functor, the adapter: +// 1. Extracts the scalar (`double`) parts of the input Jets. +// 2. Calls the inner CostFunction::Evaluate to compute the inner residuals +// and Jacobians with respect to the inner inputs. +// 3. Applies the chain rule to multiply the inner Jacobians by the derivative +// (`.v`) parts of the input Jets to form the output residual Jets. +// +// To benchmark this composition realistically, we split a standard pinhole +// camera projection with radial distortion into two stages: +// +// - Stage 1 (`InnerProjectionFunctor`): Projects a 3D point already in the +// camera coordinate frame, `p_cam = [X, Y, Z]`, into 2D pixel coordinates +// using camera `intrinsics = [focal, k1, k2]`: +// xp = X / Z, yp = Y / Z, r^2 = xp^2 + yp^2 +// distortion = 1 + k1 * r^2 + k2 * r^4 +// residuals = [focal * distortion * xp, focal * distortion * yp] +// This inner functor is wrapped in an `AutoDiffCostFunction<..., 2, 3, 3>`, +// representing a pre-existing CostFunction object. +// +// - Stage 2 (`OuterProjectionFunctor`): Takes four 3D parameter blocks +// (`rotation` angle-axis, `translation`, `intrinsics`, and a world-frame +// `point`), transforms the world point into the camera frame: +// p_cam = AngleAxisRotatePoint(rotation, point) + translation +// and then invokes the inner CostFunction via either: +// * `CostFunctionToFunctor<2, 3, 3>` (when `!kUseDynamicAdapter`), or +// * `DynamicCostFunctionToFunctor` (when `kUseDynamicAdapter`). +struct InnerProjectionFunctor { + template <typename T> + bool operator()(const T* const intrinsics, + const T* const point, + T* residuals) const { + const T xp = point[0] / point[2]; + const T yp = point[1] / point[2]; + const T r2 = xp * xp + yp * yp; + const T distortion = T(1.0) + r2 * (intrinsics[1] + intrinsics[2] * r2); + residuals[0] = intrinsics[0] * distortion * xp; + residuals[1] = intrinsics[0] * distortion * yp; + return true; + } +}; + +template <bool kUseDynamicAdapter> +struct OuterProjectionFunctor : BenchmarkCostFunctor<2, 3, 3, 3, 3> { + using typename BenchmarkCostFunctor<2, 3, 3, 3, 3>::ParameterBlocks; + + OuterProjectionFunctor() + : inner_(std::make_unique< + AutoDiffCostFunction<InnerProjectionFunctor, 2, 3, 3>>( + std::make_unique<InnerProjectionFunctor>())) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks({0.1, -0.2, 0.05}, + {0.5, -0.1, 2.0}, + {500.0, -0.01, 0.001}, + {0.3, -0.4, 5.0}); + } + + template <typename T> + bool operator()(const T* const rotation, + const T* const translation, + const T* const intrinsics, + const T* const point, + T* residuals) const { + // Transform the 3D point from the world frame into the camera frame. + T p_cam[3]; + AngleAxisRotatePoint(rotation, point, p_cam); + p_cam[0] += translation[0]; + p_cam[1] += translation[1]; + p_cam[2] += translation[2]; + + // Project the camera-frame point into 2D pixel coordinates by invoking the + // wrapped inner CostFunction through CostFunctionToFunctor (static) or + // DynamicCostFunctionToFunctor (dynamic). + if constexpr (!kUseDynamicAdapter) { + return inner_(intrinsics, p_cam, residuals); + } else { + const T* params[2] = {intrinsics, p_cam}; + return inner_(params, residuals); + } + } + + std::conditional_t<!kUseDynamicAdapter, + CostFunctionToFunctor<2, 3, 3>, + DynamicCostFunctionToFunctor> + inner_; +}; + +// 1. COLMAP ReprojErrorCostFunctor<OpenCVCameraModel>: <2, 3, 7, 8> +// point3D_in_world (3), cam_from_world [qx, qy, qz, qw, tx, ty, tz] (7), +// camera_params [fx, fy, cx, cy, k1, k2, p1, p2] (8). +struct ColmapOpenCVReprojectionError : BenchmarkCostFunctor<2, 3, 7, 8> { + static constexpr int kPointBlockIndex = 0; + + explicit ColmapOpenCVReprojectionError(double u = 650.0, double v = 350.0) + : obs_x(u), obs_y(v) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {0.5, -0.3, 4.2}, + NormalizedPose(0.01, 0.02, -0.01, 0.9997, 0.1, -0.2, 0.5), + {1000.0, 1000.0, 640.0, 360.0, -0.05, 0.01, 1e-4, -1e-4}); + } + + static ParameterBlocks MakeParametersFromBAL(const double* bal_camera, + const double* bal_point) { + // BAL cameras look down -Z with focal length bal_camera[6] and radial + // distortion (bal_camera[7], bal_camera[8]); negate focal length so + // projection matches COLMAP's +Z camera convention. + std::array<double, 8> intrinsics = {-bal_camera[6], + -bal_camera[6], + 0.0, + 0.0, + bal_camera[7], + bal_camera[8], + 1e-4, + 1e-4}; + return ParameterBlocks({bal_point[0], bal_point[1], bal_point[2]}, + PoseFromBALCamera(bal_camera), + intrinsics); + } + + static std::unique_ptr<Manifold> MakeManifold(int block_index) { + if (block_index == 1) { + return std::make_unique< + ProductManifold<EigenQuaternionManifold, EuclideanManifold<3>>>(); + } + return nullptr; + } + + static std::unique_ptr<LossFunction> MakeLossFunction() { + return std::make_unique<HuberLoss>(1.0); + } + + template <typename T> + inline bool operator()(const T* const point3D_in_world, + const T* const cam_from_world, + const T* const camera_params, + T* residuals) const { + const Eigen::Map<const Eigen::Quaternion<T>> q(cam_from_world); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(cam_from_world + 4); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w(point3D_in_world); + const Eigen::Matrix<T, 3, 1> p_c = q * p_w + t; + + const T f1 = camera_params[0]; + const T f2 = camera_params[1]; + const T c1 = camera_params[2]; + const T c2 = camera_params[3]; + const T k1 = camera_params[4]; + const T k2 = camera_params[5]; + const T p1 = camera_params[6]; + const T p2 = camera_params[7]; + + const T u = p_c[0] / p_c[2]; + const T v = p_c[1] / p_c[2]; + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k1 * r2 + k2 * r2 * r2; + const T du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2); + const T dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2); + + residuals[0] = f1 * (u + du) + c1 - T(obs_x); + residuals[1] = f2 * (v + dv) + c2 - T(obs_y); + return true; + } + + double obs_x; + double obs_y; +}; + +// 2. COLMAP ReprojErrorCostFunctor<FullOpenCVCameraModel>: <2, 3, 7, 12> +// Rational radial + tangential distortion: +// camera_params [fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6] (12). +struct ColmapFullOpenCVReprojectionError : BenchmarkCostFunctor<2, 3, 7, 12> { + static constexpr int kPointBlockIndex = 0; + + explicit ColmapFullOpenCVReprojectionError(double u = 650.0, double v = 350.0) + : obs_x(u), obs_y(v) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {0.5, -0.3, 4.2}, + NormalizedPose(0.01, 0.02, -0.01, 0.9997, 0.1, -0.2, 0.5), + {1000.0, + 1000.0, + 640.0, + 360.0, + -0.05, + 0.01, + 1e-4, + -1e-4, + 0.002, + 0.01, + -0.005, + 0.001}); + } + + template <typename T> + inline bool operator()(const T* const point3D_in_world, + const T* const cam_from_world, + const T* const camera_params, + T* residuals) const { + const Eigen::Map<const Eigen::Quaternion<T>> q(cam_from_world); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(cam_from_world + 4); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w(point3D_in_world); + const Eigen::Matrix<T, 3, 1> p_c = q * p_w + t; + + const T f1 = camera_params[0]; + const T f2 = camera_params[1]; + const T c1 = camera_params[2]; + const T c2 = camera_params[3]; + const T k1 = camera_params[4]; + const T k2 = camera_params[5]; + const T p1 = camera_params[6]; + const T p2 = camera_params[7]; + const T k3 = camera_params[8]; + const T k4 = camera_params[9]; + const T k5 = camera_params[10]; + const T k6 = camera_params[11]; + + const T u = p_c[0] / p_c[2]; + const T v = p_c[1] / p_c[2]; + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T r4 = r2 * r2; + const T r6 = r4 * r2; + const T radial = (T(1) + k1 * r2 + k2 * r4 + k3 * r6) / + (T(1) + k4 * r2 + k5 * r4 + k6 * r6); + const T x = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2); + const T y = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2); + + residuals[0] = f1 * x + c1 - T(obs_x); + residuals[1] = f2 * y + c2 - T(obs_y); + return true; + } + + double obs_x; + double obs_y; +}; + +// 3. COLMAP RigReprojErrorCostFunctor<OpenCVCameraModel>: <2, 3, 7, 7, 8> +// point3D_in_world (3), cam_from_rig (7), rig_from_world (7), camera_params +// (8). +struct ColmapRigReprojectionError : BenchmarkCostFunctor<2, 3, 7, 7, 8> { + static constexpr int kPointBlockIndex = 0; + + explicit ColmapRigReprojectionError(double u = 650.0, double v = 350.0) + : obs_x(u), obs_y(v) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {0.5, -0.3, 4.2}, + NormalizedPose(0.01, -0.01, 0.02, 0.9997, 0.05, 0.0, 0.0), + NormalizedPose(0.02, 0.01, -0.02, 0.9995, 0.1, -0.2, 0.5), + {1000.0, 1000.0, 640.0, 360.0, -0.05, 0.01, 1e-4, -1e-4}); + } + + template <typename T> + inline bool operator()(const T* const point3D_in_world, + const T* const cam_from_rig, + const T* const rig_from_world, + const T* const camera_params, + T* residuals) const { + const Eigen::Map<const Eigen::Quaternion<T>> q_cr(cam_from_rig); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_cr(cam_from_rig + 4); + const Eigen::Map<const Eigen::Quaternion<T>> q_rw(rig_from_world); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_rw(rig_from_world + 4); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w(point3D_in_world); + + const Eigen::Matrix<T, 3, 1> p_c = q_cr * (q_rw * p_w + t_rw) + t_cr; + + const T f1 = camera_params[0]; + const T f2 = camera_params[1]; + const T c1 = camera_params[2]; + const T c2 = camera_params[3]; + const T k1 = camera_params[4]; + const T k2 = camera_params[5]; + const T p1 = camera_params[6]; + const T p2 = camera_params[7]; + + const T u = p_c[0] / p_c[2]; + const T v = p_c[1] / p_c[2]; + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k1 * r2 + k2 * r2 * r2; + const T du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2); + const T dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2); + + residuals[0] = f1 * (u + du) + c1 - T(obs_x); + residuals[1] = f2 * (v + dv) + c2 - T(obs_y); + return true; + } + + double obs_x; + double obs_y; +}; + +// 4. COLMAP SampsonErrorCostFunctor: <1, 7> +// Essential matrix E = [t]_x R(q) and signed Sampson epipolar error. +struct ColmapSampsonError : BenchmarkCostFunctor<1, 7> { + explicit ColmapSampsonError( + const Eigen::Vector2d& p1 = Eigen::Vector2d(0.1, -0.2), + const Eigen::Vector2d& p2 = Eigen::Vector2d(0.15, -0.18)) + : point1_(p1), point2_(p2) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + NormalizedPose(0.01, 0.02, -0.01, 0.9997, 0.8, 0.4, 0.4472)); + } + + template <typename T> + inline bool operator()(const T* const cam2_from_cam1, T* residuals) const { + const Eigen::Matrix<T, 3, 3> R = + Eigen::Map<const Eigen::Quaternion<T>>(cam2_from_cam1) + .toRotationMatrix(); + Eigen::Matrix<T, 3, 3> t_x; + t_x << T(0), -cam2_from_cam1[6], cam2_from_cam1[5], cam2_from_cam1[6], T(0), + -cam2_from_cam1[4], -cam2_from_cam1[5], cam2_from_cam1[4], T(0); + const Eigen::Matrix<T, 3, 3> E = t_x * R; + + const Eigen::Matrix<T, 3, 1> p1(T(point1_.x()), T(point1_.y()), T(1)); + const Eigen::Matrix<T, 3, 1> p2(T(point2_.x()), T(point2_.y()), T(1)); + const Eigen::Matrix<T, 3, 1> epipolar_line1 = E * p1; + const T num = p2.dot(epipolar_line1); + const Eigen::Matrix<T, 4, 1> denom(p2.dot(E.col(0)), + p2.dot(E.col(1)), + epipolar_line1.x(), + epipolar_line1.y()); + residuals[0] = num / denom.norm(); + return true; + } + + Eigen::Vector2d point1_; + Eigen::Vector2d point2_; +}; + +// 5. COLMAP ReprojErrorCostFunctor<OpenCVFisheyeCameraModel>: <2, 3, 7, 8> +// Kannala-Brandt equidistant fisheye camera model: +// point3D_in_world (3), cam_from_world (7), +// camera_params [fx, fy, cx, cy, k1, k2, k3, k4] (8). +struct ColmapFisheyeReprojectionError : BenchmarkCostFunctor<2, 3, 7, 8> { + static constexpr int kPointBlockIndex = 0; + + explicit ColmapFisheyeReprojectionError(double u = 650.0, double v = 350.0) + : obs_x(u), obs_y(v) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {0.5, -0.3, 4.2}, + NormalizedPose(0.01, 0.02, -0.01, 0.9997, 0.1, -0.2, 0.5), + {800.0, 800.0, 640.0, 360.0, -0.02, 0.005, -0.001, 0.0002}); + } + + template <typename T> + inline bool operator()(const T* const point3D_in_world, + const T* const cam_from_world, + const T* const camera_params, + T* residuals) const { + const Eigen::Map<const Eigen::Quaternion<T>> q(cam_from_world); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(cam_from_world + 4); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w(point3D_in_world); + const Eigen::Matrix<T, 3, 1> p_c = q * p_w + t; + + const T f1 = camera_params[0]; + const T f2 = camera_params[1]; + const T c1 = camera_params[2]; + const T c2 = camera_params[3]; + const T k1 = camera_params[4]; + const T k2 = camera_params[5]; + const T k3 = camera_params[6]; + const T k4 = camera_params[7]; + + const T u = p_c[0] / p_c[2]; + const T v = p_c[1] / p_c[2]; + const T r = hypot(u, v); + T x = u; + T y = v; + if (r > T(1e-8)) { + const T theta = atan(r); + const T theta2 = theta * theta; + const T theta4 = theta2 * theta2; + const T theta6 = theta4 * theta2; + const T theta8 = theta4 * theta4; + const T thetad = theta * (T(1) + k1 * theta2 + k2 * theta4 + k3 * theta6 + + k4 * theta8); + const T scale = thetad / r; + x *= scale; + y *= scale; + } + + residuals[0] = f1 * x + c1 - T(obs_x); + residuals[1] = f2 * y + c2 - T(obs_y); + return true; + } + + double obs_x; + double obs_y; +}; + +// 6. COLMAP ReprojErrorCostFunctor<EquirectangularCameraModel>: <2, 3, 7> +// 360x180 spherical equirectangular projection using atan2 and asin. +struct ColmapSphericalReprojectionError : BenchmarkCostFunctor<2, 3, 7> { + static constexpr int kPointBlockIndex = 0; + + explicit ColmapSphericalReprojectionError(double u = 650.0, double v = 350.0) + : obs_x(u), obs_y(v) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {0.5, -0.3, 4.2}, + NormalizedPose(0.01, 0.02, -0.01, 0.9997, 0.1, -0.2, 0.5)); + } + + template <typename T> + inline bool operator()(const T* const point3D_in_world, + const T* const cam_from_world, + T* residuals) const { + const Eigen::Map<const Eigen::Quaternion<T>> q(cam_from_world); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(cam_from_world + 4); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w(point3D_in_world); + const Eigen::Matrix<T, 3, 1> p_c = q * p_w + t; + + const T norm = p_c.norm(); + const T lon = atan2(p_c[0], p_c[2]); + const T lat = asin(p_c[1] / norm); + + constexpr double kWidth = 1920.0; + constexpr double kHeight = 960.0; + constexpr double kInvTwoPi = 1.0 / (2.0 * constants::pi); + constexpr double kInvPi = 1.0 / constants::pi; + residuals[0] = T(kWidth) * (T(0.5) + lon * T(kInvTwoPi)) - T(obs_x); + residuals[1] = T(kHeight) * (T(0.5) + lat * T(kInvPi)) - T(obs_y); + return true; + } + + double obs_x; + double obs_y; +}; + +// 7. Blender libmv Brown-Conrady ReprojectionErrorApplyIntrinsics: <2, 9, 6, 3> +// intrinsics [f, px, py, k1, k2, k3, k4, p1, p2] (9), R_t [rx, ry, rz, tx, ty, +// tz] (6), X (3). +struct LibmvBrownReprojectionError : BenchmarkCostFunctor<2, 9, 6, 3> { + static constexpr int kPointBlockIndex = 2; + + explicit LibmvBrownReprojectionError(double u = 650.0, double v = 350.0) + : obs_x(u), obs_y(v) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {1000.0, 640.0, 360.0, -0.05, 0.01, -0.002, 0.0005, 1e-4, -1e-4}, + {0.02, -0.03, 0.01, 0.1, -0.2, 0.5}, + {0.5, -0.3, 4.2}); + } + + static ParameterBlocks MakeParametersFromBAL(const double* bal_camera, + const double* bal_point) { + std::array<double, 9> intrinsics = {-bal_camera[6], + 0.0, + 0.0, + bal_camera[7], + bal_camera[8], + 0.0, + 0.0, + 1e-4, + 1e-4}; + std::array<double, 6> pose{}; + std::copy_n(bal_camera, 6, pose.data()); + return ParameterBlocks( + intrinsics, pose, {bal_point[0], bal_point[1], bal_point[2]}); + } + + static std::unique_ptr<Manifold> MakeManifold(int block_index) { + if (block_index == 0) { + return std::make_unique<SubsetManifold>(9, std::vector<int>{1, 2, 5, 6}); + } + return nullptr; + } + + template <typename T> + inline bool operator()(const T* const intrinsics, + const T* const R_t, + const T* const X, + T* residuals) const { + T p[3]; + ceres::AngleAxisRotatePoint(R_t, X, p); + p[0] += R_t[3]; + p[1] += R_t[4]; + p[2] += R_t[5]; + + const T x = p[0] / p[2]; + const T y = p[1] / p[2]; + + const T focal = intrinsics[0]; + const T px = intrinsics[1]; + const T py = intrinsics[2]; + const T k1 = intrinsics[3]; + const T k2 = intrinsics[4]; + const T k3 = intrinsics[5]; + const T k4 = intrinsics[6]; + const T p1 = intrinsics[7]; + const T p2 = intrinsics[8]; + + const T x2 = x * x; + const T y2 = y * y; + const T xy2 = T(2) * x * y; + const T r2 = x2 + y2; + const T r_coeff = T(1) + (((k4 * r2 + k3) * r2 + k2) * r2 + k1) * r2; + const T tx = p1 * (r2 + T(2) * x2) + p2 * xy2; + const T ty = p2 * (r2 + T(2) * y2) + p1 * xy2; + + residuals[0] = focal * (x * r_coeff + tx) + px - T(obs_x); + residuals[1] = focal * (y * r_coeff + ty) + py - T(obs_y); + return true; + } + + double obs_x; + double obs_y; +}; + +// 8. Blender libmv Nuke ReprojectionErrorInvertIntrinsics: <2, 8, 6, 3> +// intrinsics [f, px, py, k1, k2, p1, p2, aspect] (8), R_t (6), X (3). +struct LibmvNukeInvertReprojectionError : BenchmarkCostFunctor<2, 8, 6, 3> { + static constexpr int kPointBlockIndex = 2; + + explicit LibmvNukeInvertReprojectionError(double u = 650.0, double v = 350.0) + : obs_x(u), obs_y(v) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {1000.0, 640.0, 360.0, -0.05, 0.01, 1e-4, -1e-4, 1.0}, + {0.02, -0.03, 0.01, 0.1, -0.2, 0.5}, + {0.5, -0.3, 4.2}); + } + + template <typename T> + inline bool operator()(const T* const intrinsics, + const T* const R_t, + const T* const X, + T* residuals) const { + const T focal = intrinsics[0]; + const T px = intrinsics[1]; + const T py = intrinsics[2]; + const T k1 = intrinsics[3]; + const T k2 = intrinsics[4]; + const T p1 = intrinsics[5]; + const T p2 = intrinsics[6]; + + T p[3]; + ceres::AngleAxisRotatePoint(R_t, X, p); + p[0] += R_t[3]; + p[1] += R_t[4]; + p[2] += R_t[5]; + + const T xn = p[0] / p[2]; + const T yn = p[1] / p[2]; + const T predicted_x = focal * xn + px; + const T predicted_y = focal * yn + py; + + constexpr double kHalfMaxImageSize = 960.0; + const T xd = (T(obs_x) - px) / T(kHalfMaxImageSize); + const T yd = (T(obs_y) - py) / T(kHalfMaxImageSize); + const T xd2 = xd * xd; + const T yd2 = yd * yd; + const T rd2 = xd2 + yd2; + const T rd4 = rd2 * rd2; + const T xu = xd / (T(1) + k1 * rd2 + k2 * rd4 + p1 * yd2); + const T yu = yd / (T(1) + k1 * rd2 + k2 * rd4 + p2 * xd2); + + const T obs_undistorted_x = xu * T(kHalfMaxImageSize) + px; + const T obs_undistorted_y = yu * T(kHalfMaxImageSize) + py; + + residuals[0] = predicted_x - obs_undistorted_x; + residuals[1] = predicted_y - obs_undistorted_y; + return true; + } + + double obs_x; + double obs_y; +}; + +// 9. 3D Pose Graph SLAM Error Term (PoseGraph3dErrorTerm): <6, 3, 4, 3, 4> +struct PoseGraph3dError : BenchmarkCostFunctor<6, 3, 4, 3, 4> { + explicit PoseGraph3dError( + const Eigen::Vector3d& p_ab = Eigen::Vector3d(1.0, 1.0, 1.0), + const Eigen::Quaterniond& q_ab = Eigen::Quaterniond::Identity(), + const Eigen::Matrix<double, 6, 6>& sqrt_info = + Eigen::Matrix<double, 6, 6>::Identity()) + : p_ab_measured_(p_ab), + q_ab_measured_(q_ab), + sqrt_information_(sqrt_info) {} + + static ParameterBlocks MakeParameters() { + return ParameterBlocks( + {1.0, 2.0, 3.0}, + NormalizedQuaternion(0.01, 0.02, -0.01, 0.9997), + {2.1, 3.2, 4.3}, + NormalizedQuaternion(0.03, -0.01, 0.02, 0.9993)); + } + + template <typename T> + inline bool operator()(const T* const p_a_ptr, + const T* const q_a_ptr, + const T* const p_b_ptr, + const T* const q_b_ptr, + T* residuals_ptr) const { + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr); + const Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr); + const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr); + const Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr); + + const Eigen::Quaternion<T> q_a_inverse = q_a.conjugate(); + const Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b; + const Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a); + const Eigen::Quaternion<T> delta_q = + q_ab_measured_.template cast<T>() * q_ab_estimated.conjugate(); + + Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr); + residuals.template block<3, 1>(0, 0) = + p_ab_estimated - p_ab_measured_.template cast<T>(); + residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec(); + residuals.applyOnTheLeft(sqrt_information_.template cast<T>()); + return true; + } + + Eigen::Vector3d p_ab_measured_; + Eigen::Quaterniond q_ab_measured_; + Eigen::Matrix<double, 6, 6> sqrt_information_; +}; + +} // namespace ceres::internal + +#endif // CERES_INTERNAL_BENCHMARK_COST_FUNCTIONS_H_
diff --git a/internal/ceres/cost_function_differentiation_benchmark.cc b/internal/ceres/cost_function_differentiation_benchmark.cc new file mode 100644 index 0000000..4f96f39 --- /dev/null +++ b/internal/ceres/cost_function_differentiation_benchmark.cc
@@ -0,0 +1,324 @@ +// 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. +// +// Authors: darius.rueckert@fau.de (Darius Rueckert) +// nikolaus@nikolaus-demmel.de (Nikolaus Demmel) +// sameeragarwal@google.com (Sameer Agarwal) + +#include <array> +#include <memory> +#include <utility> + +#include "absl/log/check.h" +#include "benchmark/benchmark.h" +#include "ceres/benchmark_cost_functions.h" +#include "ceres/ceres.h" + +namespace ceres::internal { +namespace { + +enum DiffType { + kAutoDiff, + kDynamicAutoDiff, + kAutoDiffDynamicResiduals, + kNumericForward, + kDynamicNumericForward, + kNumericCentral, + kDynamicNumericCentral, + kNumericRidders, + kDynamicNumericRidders, +}; + +// Controls which outputs CostFunction::Evaluate is called to compute: +// kResidualsOnly: evaluate residuals only (jacobians = nullptr) +// kResidualsAndJacobians: evaluate residuals and all parameter Jacobians +// kResidualsAndPointJacobian: evaluate residuals and only the 3D point +// Jacobian (at index CostFunctor::kPointBlockIndex) +enum EvaluationType { + kResidualsOnly, + kResidualsAndJacobians, + kResidualsAndPointJacobian, +}; + +template <DiffType kDiffType> +constexpr NumericDiffMethodType NumericDiffMethod() { + if constexpr (kDiffType == kNumericForward || + kDiffType == kDynamicNumericForward) { + return FORWARD; + } else if constexpr (kDiffType == kNumericCentral || + kDiffType == kDynamicNumericCentral) { + return CENTRAL; + } else if constexpr (kDiffType == kNumericRidders || + kDiffType == kDynamicNumericRidders) { + return RIDDERS; + } +} + +template <DiffType kDiffType> +constexpr bool IsDynamicNumericDiff() { + return kDiffType == kDynamicNumericForward || + kDiffType == kDynamicNumericCentral || + kDiffType == kDynamicNumericRidders; +} + +template <typename CostFunctor, DiffType kDiffType> +std::unique_ptr<CostFunction> CreateCostFunction() { + if constexpr (kDiffType == kAutoDiff) { + return CostFunctor::template CreateAutoDiff<CostFunctor>(); + } else if constexpr (kDiffType == kAutoDiffDynamicResiduals) { + return CostFunctor::template CreateAutoDiffDynamicResiduals<CostFunctor>(); + } else if constexpr (kDiffType == kDynamicAutoDiff) { + return CostFunctor::template CreateDynamicAutoDiff<CostFunctor>(); + } else if constexpr (IsDynamicNumericDiff<kDiffType>()) { + return CostFunctor::template CreateDynamicNumericDiff< + CostFunctor, + NumericDiffMethod<kDiffType>()>(); + } else { + return CostFunctor::template CreateNumericDiff< + CostFunctor, + NumericDiffMethod<kDiffType>()>(); + } +} + +// Benchmarks CostFunction::Evaluate for `CostFunctor` wrapped with `kDiffType` +// and evaluated in mode `kEvalType`. +template <typename CostFunctor, DiffType kDiffType, EvaluationType kEvalType> +void BM_CostFunction(benchmark::State& state) { + const std::unique_ptr<CostFunction> cost_function = + CreateCostFunction<CostFunctor, kDiffType>(); + const auto parameter_storage = CostFunctor::MakeParameters(); + const auto parameters = parameter_storage.Pointers(); + + double residuals[CostFunctor::kNumResiduals] = {}; + double jacobian_storage[CostFunctor::kNumResiduals * + CostFunctor::kTotalParameters] = {}; + double* jacobians[CostFunctor::kNumParameterBlocks] = {}; + + double* cursor = jacobian_storage; + for (int i = 0; i < CostFunctor::kNumParameterBlocks; ++i) { + if constexpr (kEvalType == kResidualsAndJacobians) { + jacobians[i] = cursor; + } else if constexpr (kEvalType == kResidualsAndPointJacobian) { + static_assert(CostFunctor::kPointBlockIndex >= 0 && + CostFunctor::kPointBlockIndex < + CostFunctor::kNumParameterBlocks); + if (i == CostFunctor::kPointBlockIndex) { + jacobians[i] = cursor; + } + } + cursor += CostFunctor::kNumResiduals * CostFunctor::kBlockSizes[i]; + } + double** jacobians_ptr = (kEvalType == kResidualsOnly) ? nullptr : jacobians; + + CHECK(cost_function->Evaluate(parameters.data(), residuals, jacobians_ptr)); + + for (auto _ : state) { + benchmark::DoNotOptimize( + cost_function->Evaluate(parameters.data(), residuals, jacobians_ptr)); + benchmark::DoNotOptimize(residuals); + benchmark::DoNotOptimize(jacobian_storage); + } +} + +#define REGISTER_AUTODIFF_BENCHMARKS(Functor) \ + BENCHMARK_TEMPLATE(BM_CostFunction, Functor, kAutoDiff, kResidualsOnly); \ + BENCHMARK_TEMPLATE( \ + BM_CostFunction, Functor, kAutoDiff, kResidualsAndJacobians); \ + BENCHMARK_TEMPLATE( \ + BM_CostFunction, Functor, kDynamicAutoDiff, kResidualsOnly); \ + BENCHMARK_TEMPLATE( \ + BM_CostFunction, Functor, kDynamicAutoDiff, kResidualsAndJacobians) + +#define REGISTER_CONSTANT_BENCHMARKS(N) \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + ConstantCostFunction<N>, \ + kAutoDiff, \ + kResidualsAndJacobians); \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + ConstantCostFunction<N>, \ + kDynamicAutoDiff, \ + kResidualsAndJacobians) + +REGISTER_CONSTANT_BENCHMARKS(1); +REGISTER_CONSTANT_BENCHMARKS(10); +REGISTER_CONSTANT_BENCHMARKS(20); +REGISTER_CONSTANT_BENCHMARKS(30); +REGISTER_CONSTANT_BENCHMARKS(40); +REGISTER_CONSTANT_BENCHMARKS(50); +REGISTER_CONSTANT_BENCHMARKS(60); + +#undef REGISTER_CONSTANT_BENCHMARKS + +REGISTER_AUTODIFF_BENCHMARKS(LinearCostFunction<1>); +REGISTER_AUTODIFF_BENCHMARKS(LinearCostFunction<10>); + +#define REGISTER_RAT43_BENCHMARKS(Diff) \ + BENCHMARK_TEMPLATE( \ + BM_CostFunction, Rat43CostFunctor, Diff, kResidualsOnly); \ + BENCHMARK_TEMPLATE( \ + BM_CostFunction, Rat43CostFunctor, Diff, kResidualsAndJacobians) + +REGISTER_RAT43_BENCHMARKS(kAutoDiff); +REGISTER_RAT43_BENCHMARKS(kDynamicAutoDiff); +REGISTER_RAT43_BENCHMARKS(kNumericForward); +REGISTER_RAT43_BENCHMARKS(kDynamicNumericForward); +REGISTER_RAT43_BENCHMARKS(kNumericCentral); +REGISTER_RAT43_BENCHMARKS(kDynamicNumericCentral); +REGISTER_RAT43_BENCHMARKS(kNumericRidders); +REGISTER_RAT43_BENCHMARKS(kDynamicNumericRidders); + +#undef REGISTER_RAT43_BENCHMARKS + +#define REGISTER_SNAVELY_ALL_MODES(Diff) \ + BENCHMARK_TEMPLATE( \ + BM_CostFunction, SnavelyReprojectionError, Diff, kResidualsOnly); \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + SnavelyReprojectionError, \ + Diff, \ + kResidualsAndJacobians); \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + SnavelyReprojectionError, \ + Diff, \ + kResidualsAndPointJacobian) + +REGISTER_SNAVELY_ALL_MODES(kAutoDiff); +REGISTER_SNAVELY_ALL_MODES(kDynamicAutoDiff); +BENCHMARK_TEMPLATE(BM_CostFunction, + SnavelyReprojectionError, + kAutoDiffDynamicResiduals, + kResidualsOnly); +BENCHMARK_TEMPLATE(BM_CostFunction, + SnavelyReprojectionError, + kAutoDiffDynamicResiduals, + kResidualsAndJacobians); +REGISTER_SNAVELY_ALL_MODES(kNumericForward); +REGISTER_SNAVELY_ALL_MODES(kDynamicNumericForward); +REGISTER_SNAVELY_ALL_MODES(kNumericCentral); +REGISTER_SNAVELY_ALL_MODES(kDynamicNumericCentral); +BENCHMARK_TEMPLATE( + BM_CostFunction, SnavelyReprojectionError, kNumericRidders, kResidualsOnly); +BENCHMARK_TEMPLATE(BM_CostFunction, + SnavelyReprojectionError, + kNumericRidders, + kResidualsAndJacobians); +BENCHMARK_TEMPLATE(BM_CostFunction, + SnavelyReprojectionError, + kDynamicNumericRidders, + kResidualsOnly); +BENCHMARK_TEMPLATE(BM_CostFunction, + SnavelyReprojectionError, + kDynamicNumericRidders, + kResidualsAndJacobians); + +#undef REGISTER_SNAVELY_ALL_MODES + +REGISTER_AUTODIFF_BENCHMARKS(PhotometricError<8>); + +REGISTER_AUTODIFF_BENCHMARKS(RelativePoseError); +BENCHMARK_TEMPLATE( + BM_CostFunction, RelativePoseError, kNumericCentral, kResidualsOnly); +BENCHMARK_TEMPLATE(BM_CostFunction, + RelativePoseError, + kNumericCentral, + kResidualsAndJacobians); +BENCHMARK_TEMPLATE( + BM_CostFunction, RelativePoseError, kDynamicNumericCentral, kResidualsOnly); +BENCHMARK_TEMPLATE(BM_CostFunction, + RelativePoseError, + kDynamicNumericCentral, + kResidualsAndJacobians); + +REGISTER_AUTODIFF_BENCHMARKS(Brdf); + +BENCHMARK_TEMPLATE(BM_CostFunction, + OuterProjectionFunctor<false>, + kAutoDiff, + kResidualsOnly); +BENCHMARK_TEMPLATE(BM_CostFunction, + OuterProjectionFunctor<false>, + kAutoDiff, + kResidualsAndJacobians); +BENCHMARK_TEMPLATE(BM_CostFunction, + OuterProjectionFunctor<true>, + kAutoDiff, + kResidualsOnly); +BENCHMARK_TEMPLATE(BM_CostFunction, + OuterProjectionFunctor<true>, + kAutoDiff, + kResidualsAndJacobians); + +#define REGISTER_REAL_WORLD_BM(Functor) \ + REGISTER_AUTODIFF_BENCHMARKS(Functor); \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + Functor, \ + kNumericCentral, \ + kResidualsAndJacobians); \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + Functor, \ + kDynamicNumericCentral, \ + kResidualsAndJacobians) + +#define REGISTER_REAL_WORLD_POINT_JAC_BM(Functor) \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + Functor, \ + kAutoDiff, \ + kResidualsAndPointJacobian); \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + Functor, \ + kDynamicAutoDiff, \ + kResidualsAndPointJacobian); \ + BENCHMARK_TEMPLATE(BM_CostFunction, \ + Functor, \ + kNumericCentral, \ + kResidualsAndPointJacobian) + +REGISTER_REAL_WORLD_BM(ColmapOpenCVReprojectionError); +REGISTER_REAL_WORLD_POINT_JAC_BM(ColmapOpenCVReprojectionError); +REGISTER_REAL_WORLD_BM(ColmapFullOpenCVReprojectionError); +REGISTER_REAL_WORLD_POINT_JAC_BM(ColmapFullOpenCVReprojectionError); +REGISTER_REAL_WORLD_BM(ColmapRigReprojectionError); +REGISTER_REAL_WORLD_POINT_JAC_BM(ColmapRigReprojectionError); +REGISTER_REAL_WORLD_BM(ColmapSampsonError); +REGISTER_REAL_WORLD_BM(ColmapFisheyeReprojectionError); +REGISTER_REAL_WORLD_POINT_JAC_BM(ColmapFisheyeReprojectionError); +REGISTER_REAL_WORLD_BM(ColmapSphericalReprojectionError); +REGISTER_REAL_WORLD_POINT_JAC_BM(ColmapSphericalReprojectionError); +REGISTER_REAL_WORLD_BM(LibmvBrownReprojectionError); +REGISTER_REAL_WORLD_POINT_JAC_BM(LibmvBrownReprojectionError); +REGISTER_REAL_WORLD_BM(LibmvNukeInvertReprojectionError); +REGISTER_REAL_WORLD_POINT_JAC_BM(LibmvNukeInvertReprojectionError); +REGISTER_REAL_WORLD_BM(PoseGraph3dError); + +#undef REGISTER_REAL_WORLD_POINT_JAC_BM +#undef REGISTER_REAL_WORLD_BM +#undef REGISTER_AUTODIFF_BENCHMARKS + +} // namespace +} // namespace ceres::internal + +BENCHMARK_MAIN();