// 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 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;
  }
};

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;
}

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.});
  }

  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});
  }

  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});
  }

  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_