Add photometric and relative-pose residuals to autodiff benchmarks
Change-Id: Id100ff2656ab63bb4fd19a51b95e78281cfd8b4a
diff --git a/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc b/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc
index c274aa5..38b3632 100644
--- a/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc
+++ b/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc
@@ -29,11 +29,14 @@
// Author: darius.rueckert@fau.de (Darius Rueckert)
#include <memory>
+#include <random>
#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"
#include "ceres/codegen/test_utils.h"
@@ -270,6 +273,100 @@
BENCHMARK(BM_SnavelyReprojectionAutoDiff)->Arg(0)->Arg(1);
+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(
+ new ceres::AutoDiffCostFunction<FunctorType,
+ FunctorType::PATCH_SIZE,
+ FunctorType::POSE_SIZE,
+ FunctorType::POSE_SIZE,
+ FunctorType::POINT_SIZE>(new FunctorType(
+ intensities_host, bearings_host, image_target, intrinsics)));
+
+ for (auto _ : state) {
+ cost_function->Evaluate(
+ parameters, residuals, state.range(0) ? jacobians : nullptr);
+ }
+}
+
+BENCHMARK(BM_PhotometricAutoDiff)->Arg(0)->Arg(1);
+
+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(
+ new ceres::AutoDiffCostFunction<FunctorType, 6, 7, 7>(
+ new FunctorType(q_i_j, t_i_j)));
+
+ for (auto _ : state) {
+ cost_function->Evaluate(
+ parameters, residuals, state.range(0) ? jacobians : nullptr);
+ }
+}
+
+BENCHMARK(BM_RelativePoseAutoDiff)->Arg(0)->Arg(1);
+
#ifdef WITH_CODE_GENERATION
static void BM_BrdfCodeGen(benchmark::State& state) {
using FunctorType = ceres::internal::CostFunctionToFunctor<Brdf>;
diff --git a/internal/ceres/autodiff_benchmarks/photometric_error.h b/internal/ceres/autodiff_benchmarks/photometric_error.h
new file mode 100644
index 0000000..9107ca0
--- /dev/null
+++ b/internal/ceres/autodiff_benchmarks/photometric_error.h
@@ -0,0 +1,191 @@
+// 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>
+ 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>
+ 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 throw an exception to indicate
+ // immediately if it does actually happen after possible future changes.
+ throw std::runtime_error("Benchmark data leads to invalid projection.");
+ }
+
+ // 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
new file mode 100644
index 0000000..93b28c7
--- /dev/null
+++ b/internal/ceres/autodiff_benchmarks/relative_pose_error.h
@@ -0,0 +1,87 @@
+// 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 "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(const Eigen::Quaterniond& q_i_j,
+ const Eigen::Vector3d& t_i_j)
+ : meas_q_i_j_(q_i_j), meas_t_i_j_(t_i_j) {}
+
+ template <typename T>
+ 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_
