From 8c8738bf832f0fc27f0d4a9585fc59b2eaa6a828 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Sun, 29 Mar 2020 13:29:02 +0200 Subject: [PATCH] Add photometric and relative-pose residuals to autodiff benchmarks Change-Id: Id100ff2656ab63bb4fd19a51b95e78281cfd8b4a --- .../autodiff_benchmarks.cc | 97 +++++++++ .../autodiff_benchmarks/photometric_error.h | 191 ++++++++++++++++++ .../autodiff_benchmarks/relative_pose_error.h | 87 ++++++++ 3 files changed, 375 insertions(+) create mode 100644 internal/ceres/autodiff_benchmarks/photometric_error.h create mode 100644 internal/ceres/autodiff_benchmarks/relative_pose_error.h diff --git a/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc b/internal/ceres/autodiff_benchmarks/autodiff_benchmarks.cc index c274aa5a5..38b3632d0 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 +#include #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 @@ static void BM_SnavelyReprojectionAutoDiff(benchmark::State& state) { BENCHMARK(BM_SnavelyReprojectionAutoDiff)->Arg(0)->Arg(1); +static void BM_PhotometricAutoDiff(benchmark::State& state) { + constexpr int PATCH_SIZE = 8; + + using FunctorType = PhotometricError; + using ImageType = Eigen::Matrix; + + // 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(parameter_block1).normalize(); + Eigen::Map(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 uniform01(0.0, 1.0); + std::uniform_int_distribution uniform0255(0, 255); + + FunctorType::Patch intensities_host = + FunctorType::Patch::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 bearings_host = + FunctorType::PatchVectors::NullaryExpr( + [&]() { return uniform01(gen); }); + bearings_host.row(2).array() = 1; + bearings_host.colwise().normalize(); + + ImageType image = ImageType::NullaryExpr( + [&]() { return static_cast(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 cost_function( + new ceres::AutoDiffCostFunction(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(parameter_block1).normalize(); + Eigen::Map(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 cost_function( + new ceres::AutoDiffCostFunction( + 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; diff --git a/internal/ceres/autodiff_benchmarks/photometric_error.h b/internal/ceres/autodiff_benchmarks/photometric_error.h new file mode 100644 index 000000000..9107ca0f9 --- /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 + +#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 +struct PhotometricError { + static constexpr int PATCH_SIZE = PATCH_SIZE_; + static constexpr int POSE_SIZE = 7; + static constexpr int POINT_SIZE = 1; + + using Grid = Grid2D; + using Interpolator = BiCubicInterpolator; + using Intrinsics = Eigen::Array; + + template + using Patch = Eigen::Array; + + template + using PatchVectors = Eigen::Matrix; + + PhotometricError(const Patch& intensities_host, + const PatchVectors& bearings_host, + const Interpolator& image_target, + const Intrinsics& intrinsics) + : intensities_host_(intensities_host), + bearings_host_(bearings_host), + image_target_(image_target), + intrinsics_(intrinsics) {} + + template + bool Project(Eigen::Matrix& proj, + const Eigen::Matrix& 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 + 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> q_w_h(pose_host_ptr); + Eigen::Map> t_w_h(pose_host_ptr + 4); + Eigen::Map> q_w_t(pose_target_ptr); + Eigen::Map> t_w_t(pose_target_ptr + 4); + const T& idist = *idist_ptr; + Eigen::Map> residuals(residuals_ptr); + + // Compute relative pose from host to target frame. + const Eigen::Quaternion q_t_h = q_w_t.conjugate() * q_w_h; + const Eigen::Matrix R_t_h = q_t_h.toRotationMatrix(); + const Eigen::Matrix 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 p_target_scaled = + (R_t_h * bearings_host_).colwise() + idist * t_t_h; + + // Project points and interpolate image. + Patch intensities_target; + for (int i = 0; i < p_target_scaled.cols(); ++i) { + Eigen::Matrix uv; + if (!Project(uv, Eigen::Matrix(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& intensities_host_; + const PatchVectors& 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 000000000..93b28c74e --- /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 + +#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 + bool operator()(const T* const pose_i_ptr, + const T* const pose_j_ptr, + T* residuals_ptr) const { + Eigen::Map> q_w_i(pose_i_ptr); + Eigen::Map> t_w_i(pose_i_ptr + 4); + Eigen::Map> q_w_j(pose_j_ptr); + Eigen::Map> t_w_j(pose_j_ptr + 4); + Eigen::Map> residuals(residuals_ptr); + + // Compute estimate of relative pose from i to j. + const Eigen::Quaternion est_q_j_i = q_w_j.conjugate() * q_w_i; + const Eigen::Matrix est_t_j_i = + q_w_j.conjugate() * (t_w_i - t_w_j); + + // Compute residual pose. + const Eigen::Quaternion res_q = meas_q_i_j_.cast() * est_q_j_i; + const Eigen::Matrix res_t = + meas_q_i_j_.cast() * est_t_j_i + meas_t_i_j_; + + // Convert quaternion to ceres convention (Eigen stores xyzw, Ceres wxyz). + Eigen::Matrix 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_