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_