Add example for BiCubicInterpolator Add example of BiCubicInterpolator usage, for both analytic and automatic differentiation Change-Id: I2e52bec5e0721a21a789e714a126ea24b1c0ce8b
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index a43c8aa..9959563 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt
@@ -76,6 +76,12 @@ add_executable(simple_bundle_adjuster simple_bundle_adjuster.cc) target_link_libraries(simple_bundle_adjuster Ceres::ceres) +add_executable(bicubic_interpolation bicubic_interpolation.cc) +target_link_libraries(bicubic_interpolation Ceres::ceres) + +add_executable(bicubic_interpolation_analytic bicubic_interpolation_analytic.cc) +target_link_libraries(bicubic_interpolation_analytic Ceres::ceres) + if (GFLAGS) add_executable(powell powell.cc) target_link_libraries(powell Ceres::ceres gflags)
diff --git a/examples/bicubic_interpolation.cc b/examples/bicubic_interpolation.cc new file mode 100644 index 0000000..f097436 --- /dev/null +++ b/examples/bicubic_interpolation.cc
@@ -0,0 +1,153 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2021 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. +// +// Bicubic interpolation with automatic differentiation +// +// We will use estimation of 2d shift as a sample problem for bicubic +// interpolation. +// +// Let us define f(x, y) = x * x - y * x + y * y +// And optimize cost function sum_i [f(x_i + s_x, y_i + s_y) - v_i]^2 +// +// Bicubic interpolation of f(x, y) will be exact, thus we can expect close to +// perfect convergence + +#include "ceres/ceres.h" +#include "ceres/cubic_interpolation.h" +#include "glog/logging.h" + +using Grid = ceres::Grid2D<double>; +using Interpolator = ceres::BiCubicInterpolator<Grid>; + +// Cost-function using autodiff interface of BiCubicInterpolator +struct AutoDiffBiCubicCost { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + template <typename T> + bool operator()(const T* s, T* residual) const { + using Vector2T = Eigen::Matrix<T, 2, 1>; + Eigen::Map<const Vector2T> shift(s); + + const Vector2T point = point_ + shift; + + T v; + interpolator_.Evaluate(point.y(), point.x(), &v); + + *residual = v - value_; + return true; + } + + AutoDiffBiCubicCost(const Interpolator& interpolator, + const Eigen::Vector2d& point, + double value) + : point_(point), value_(value), interpolator_(interpolator) {} + + static ceres::CostFunction* Create(const Interpolator& interpolator, + const Eigen::Vector2d& point, + double value) { + return new ceres::AutoDiffCostFunction<AutoDiffBiCubicCost, 1, 2>( + new AutoDiffBiCubicCost(interpolator, point, value)); + } + + const Eigen::Vector2d point_; + const double value_; + const Interpolator& interpolator_; +}; + +// Function for input data generation +static double f(const double& x, const double& y) { + return x * x - y * x + y * y; +} + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + // Problem sizes + const int kGridRowsHalf = 9; + const int kGridColsHalf = 11; + const int kGridRows = 2 * kGridRowsHalf + 1; + const int kGridCols = 2 * kGridColsHalf + 1; + const int kPoints = 4; + + const Eigen::Vector2d shift(1.234, 2.345); + const std::array<Eigen::Vector2d, kPoints> points = { + Eigen::Vector2d{-2., -3.}, + Eigen::Vector2d{-2., 3.}, + Eigen::Vector2d{2., 3.}, + Eigen::Vector2d{2., -3.}}; + + // Data is a row-major array of kGridRows x kGridCols values of function + // f(x, y) on the grid, with x in {-kGridColsHalf, ..., +kGridColsHalf}, + // and y in {-kGridRowsHalf, ..., +kGridRowsHalf} + double data[kGridRows * kGridCols]; + for (int i = 0; i < kGridRows; ++i) { + for (int j = 0; j < kGridCols; ++j) { + // Using row-major order + int index = i * kGridCols + j; + double y = i - kGridRowsHalf; + double x = j - kGridColsHalf; + + data[index] = f(x, y); + } + } + const Grid grid(data, + -kGridRowsHalf, + kGridRowsHalf + 1, + -kGridColsHalf, + kGridColsHalf + 1); + const Interpolator interpolator(grid); + + Eigen::Vector2d shift_estimate(3.1415, 1.337); + + ceres::Problem problem; + problem.AddParameterBlock(shift_estimate.data(), 2); + + for (const auto& p : points) { + const Eigen::Vector2d shifted = p + shift; + + const double v = f(shifted.x(), shifted.y()); + problem.AddResidualBlock(AutoDiffBiCubicCost::Create(interpolator, p, v), + nullptr, + shift_estimate.data()); + } + + ceres::Solver::Options options; + options.minimizer_progress_to_stdout = true; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + std::cout << summary.BriefReport() << '\n'; + + std::cout << "Bicubic interpolation with automatic derivatives:\n"; + std::cout << "Estimated shift: " << shift_estimate.transpose() + << ", ground-truth: " << shift.transpose() + << " (error: " << (shift_estimate - shift).transpose() << ")" + << std::endl; + + CHECK_LT((shift_estimate - shift).norm(), 1e-9); + return 0; +}
diff --git a/examples/bicubic_interpolation_analytic.cc b/examples/bicubic_interpolation_analytic.cc new file mode 100644 index 0000000..84c99bb --- /dev/null +++ b/examples/bicubic_interpolation_analytic.cc
@@ -0,0 +1,164 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2021 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. +// +// Bicubic interpolation with analytic differentiation +// +// We will use estimation of 2d shift as a sample problem for bicubic +// interpolation. +// +// Let us define f(x, y) = x * x - y * x + y * y +// And optimize cost function sum_i [f(x_i + s_x, y_i + s_y) - v_i]^2 +// +// Bicubic interpolation of f(x, y) will be exact, thus we can expect close to +// perfect convergence + +#include "ceres/ceres.h" +#include "ceres/cubic_interpolation.h" +#include "glog/logging.h" + +using Grid = ceres::Grid2D<double>; +using Interpolator = ceres::BiCubicInterpolator<Grid>; + +// Cost-function using analytic interface of BiCubicInterpolator +struct AnalyticBiCubicCost : public ceres::CostFunction { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + Eigen::Map<const Eigen::Vector2d> shift(parameters[0]); + + const Eigen::Vector2d point = point_ + shift; + + double* f = residuals; + double* dfdr = nullptr; + double* dfdc = nullptr; + if (jacobians && jacobians[0]) { + dfdc = jacobians[0]; + dfdr = dfdc + 1; + } + + interpolator_.Evaluate(point.y(), point.x(), f, dfdr, dfdc); + + if (residuals) { + *f -= value_; + } + return true; + } + + AnalyticBiCubicCost(const Interpolator& interpolator, + const Eigen::Vector2d& point, + double value) + : point_(point), value_(value), interpolator_(interpolator) { + set_num_residuals(1); + *mutable_parameter_block_sizes() = {2}; + } + + static ceres::CostFunction* Create(const Interpolator& interpolator, + const Eigen::Vector2d& point, + double value) { + return new AnalyticBiCubicCost(interpolator, point, value); + } + + const Eigen::Vector2d point_; + const double value_; + const Interpolator& interpolator_; +}; + +// Function for input data generation +static double f(const double& x, const double& y) { + return x * x - y * x + y * y; +} + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + // Problem sizes + const int kGridRowsHalf = 9; + const int kGridColsHalf = 11; + const int kGridRows = 2 * kGridRowsHalf + 1; + const int kGridCols = 2 * kGridColsHalf + 1; + const int kPoints = 4; + + const Eigen::Vector2d shift(1.234, 2.345); + const std::array<Eigen::Vector2d, kPoints> points = { + Eigen::Vector2d{-2., -3.}, + Eigen::Vector2d{-2., 3.}, + Eigen::Vector2d{2., 3.}, + Eigen::Vector2d{2., -3.}}; + + // Data is a row-major array of kGridRows x kGridCols values of function + // f(x, y) on the grid, with x in {-kGridColsHalf, ..., +kGridColsHalf}, + // and y in {-kGridRowsHalf, ..., +kGridRowsHalf} + double data[kGridRows * kGridCols]; + for (int i = 0; i < kGridRows; ++i) { + for (int j = 0; j < kGridCols; ++j) { + // Using row-major order + int index = i * kGridCols + j; + double y = i - kGridRowsHalf; + double x = j - kGridColsHalf; + + data[index] = f(x, y); + } + } + const Grid grid(data, + -kGridRowsHalf, + kGridRowsHalf + 1, + -kGridColsHalf, + kGridColsHalf + 1); + const Interpolator interpolator(grid); + + Eigen::Vector2d shift_estimate(3.1415, 1.337); + + ceres::Problem problem; + problem.AddParameterBlock(shift_estimate.data(), 2); + + for (const auto& p : points) { + const Eigen::Vector2d shifted = p + shift; + + const double v = f(shifted.x(), shifted.y()); + problem.AddResidualBlock(AnalyticBiCubicCost::Create(interpolator, p, v), + nullptr, + shift_estimate.data()); + } + + ceres::Solver::Options options; + options.minimizer_progress_to_stdout = true; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + std::cout << summary.BriefReport() << '\n'; + + std::cout << "Bicubic interpolation with analytic derivatives:\n"; + std::cout << "Estimated shift: " << shift_estimate.transpose() + << ", ground-truth: " << shift.transpose() + << " (error: " << (shift_estimate - shift).transpose() << ")" + << std::endl; + + CHECK_LT((shift_estimate - shift).norm(), 1e-9); + return 0; +}