internal/ceres/cubic_interpolation_test.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   4 //
   5 // Redistribution and use in source and binary forms, with or without
   6 // modification, are permitted provided that the following conditions are met:
   7 //
   8 // * Redistributions of source code must retain the above copyright notice,
   9 //   this list of conditions and the following disclaimer.
  10 // * Redistributions in binary form must reproduce the above copyright notice,
  11 //   this list of conditions and the following disclaimer in the documentation
  12 //   and/or other materials provided with the distribution.
  13 // * Neither the name of Google Inc. nor the names of its contributors may be
  14 //   used to endorse or promote products derived from this software without
  15 //   specific prior written permission.
  16 //
  17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27 // POSSIBILITY OF SUCH DAMAGE.
  28 //
  29 // Author: sameeragarwal@google.com (Sameer Agarwal)
  30
  31 #include "ceres/cubic_interpolation.h"
  32
  33 #include "ceres/jet.h"
  34 #include "ceres/internal/scoped_ptr.h"
  35 #include "glog/logging.h"
  36 #include "gtest/gtest.h"
  37
  38 namespace ceres {
  39 namespace internal {
  40
  41 static const double kTolerance = 1e-12;
  42
  43 TEST(Grid1D, OneDataDimension) {
  44   int x[] = {1, 2, 3};
  45   Grid1D<int, 1> grid(x, 0, 3);
  46   for (int i = 0; i < 3; ++i) {
  47     double value;
  48     grid.GetValue(i, &value);
  49     EXPECT_EQ(value, static_cast<double>(i + 1));
  50   }
  51 }
  52
  53 TEST(Grid1D, OneDataDimensionOutOfBounds) {
  54   int x[] = {1, 2, 3};
  55   Grid1D<int, 1> grid(x, 0, 3);
  56   double value;
  57   grid.GetValue(-1, &value);
  58   EXPECT_EQ(value, x[0]);
  59   grid.GetValue(-2, &value);
  60   EXPECT_EQ(value, x[0]);
  61   grid.GetValue(3, &value);
  62   EXPECT_EQ(value, x[2]);
  63   grid.GetValue(4, &value);
  64   EXPECT_EQ(value, x[2]);
  65 }
  66
  67 TEST(Grid1D, TwoDataDimensionIntegerDataInterleaved) {
  68   int x[] = {1, 5,
  69              2, 6,
  70              3, 7};
  71
  72   Grid1D<int, 2, true> grid(x, 0, 3);
  73   for (int i = 0; i < 3; ++i) {
  74     double value[2];
  75     grid.GetValue(i, value);
  76     EXPECT_EQ(value[0], static_cast<double>(i + 1));
  77     EXPECT_EQ(value[1], static_cast<double>(i + 5));
  78   }
  79 }
  80
  81
  82 TEST(Grid1D, TwoDataDimensionIntegerDataStacked) {
  83   int x[] = {1, 2, 3,
  84              5, 6, 7};
  85
  86   Grid1D<int, 2, false> grid(x, 0, 3);
  87   for (int i = 0; i < 3; ++i) {
  88     double value[2];
  89     grid.GetValue(i, value);
  90     EXPECT_EQ(value[0], static_cast<double>(i + 1));
  91     EXPECT_EQ(value[1], static_cast<double>(i + 5));
  92   }
  93 }
  94
  95 TEST(Grid2D, OneDataDimensionRowMajor) {
  96   int x[] = {1, 2, 3,
  97              2, 3, 4};
  98   Grid2D<int, 1, true, true> grid(x, 0, 2, 0, 3);
  99   for (int r = 0; r < 2; ++r) {
 100     for (int c = 0; c < 3; ++c) {
 101       double value;
 102       grid.GetValue(r, c, &value);
 103       EXPECT_EQ(value, static_cast<double>(r + c + 1));
 104     }
 105   }
 106 }
 107
 108 TEST(Grid2D, OneDataDimensionRowMajorOutOfBounds) {
 109   int x[] = {1, 2, 3,
 110              2, 3, 4};
 111   Grid2D<int, 1, true, true> grid(x, 0, 2, 0, 3);
 112   double value;
 113   grid.GetValue(-1, -1, &value);
 114   EXPECT_EQ(value, x[0]);
 115   grid.GetValue(-1, 0, &value);
 116   EXPECT_EQ(value, x[0]);
 117   grid.GetValue(-1, 1, &value);
 118   EXPECT_EQ(value, x[1]);
 119   grid.GetValue(-1, 2, &value);
 120   EXPECT_EQ(value, x[2]);
 121   grid.GetValue(-1, 3, &value);
 122   EXPECT_EQ(value, x[2]);
 123   grid.GetValue(0, 3, &value);
 124   EXPECT_EQ(value, x[2]);
 125   grid.GetValue(1, 3, &value);
 126   EXPECT_EQ(value, x[5]);
 127   grid.GetValue(2, 3, &value);
 128   EXPECT_EQ(value, x[5]);
 129   grid.GetValue(2, 2, &value);
 130   EXPECT_EQ(value, x[5]);
 131   grid.GetValue(2, 1, &value);
 132   EXPECT_EQ(value, x[4]);
 133   grid.GetValue(2, 0, &value);
 134   EXPECT_EQ(value, x[3]);
 135   grid.GetValue(2, -1, &value);
 136   EXPECT_EQ(value, x[3]);
 137   grid.GetValue(1, -1, &value);
 138   EXPECT_EQ(value, x[3]);
 139   grid.GetValue(0, -1, &value);
 140   EXPECT_EQ(value, x[0]);
 141 }
 142
 143 TEST(Grid2D, TwoDataDimensionRowMajorInterleaved) {
 144   int x[] = {1, 4, 2, 8, 3, 12,
 145              2, 8, 3, 12, 4, 16};
 146   Grid2D<int, 2, true, true> grid(x, 0, 2, 0, 3);
 147   for (int r = 0; r < 2; ++r) {
 148     for (int c = 0; c < 3; ++c) {
 149       double value[2];
 150       grid.GetValue(r, c, value);
 151       EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
 152       EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
 153     }
 154   }
 155 }
 156
 157 TEST(Grid2D, TwoDataDimensionRowMajorStacked) {
 158   int x[] = {1,  2,  3,
 159              2,  3,  4,
 160              4,  8, 12,
 161              8, 12, 16};
 162   Grid2D<int, 2, true, false> grid(x, 0, 2, 0, 3);
 163   for (int r = 0; r < 2; ++r) {
 164     for (int c = 0; c < 3; ++c) {
 165       double value[2];
 166       grid.GetValue(r, c, value);
 167       EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
 168       EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
 169     }
 170   }
 171 }
 172
 173 TEST(Grid2D, TwoDataDimensionColMajorInterleaved) {
 174   int x[] = { 1,  4, 2,  8,
 175               2,  8, 3, 12,
 176               3, 12, 4, 16};
 177   Grid2D<int, 2, false, true> grid(x, 0, 2, 0, 3);
 178   for (int r = 0; r < 2; ++r) {
 179     for (int c = 0; c < 3; ++c) {
 180       double value[2];
 181       grid.GetValue(r, c, value);
 182       EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
 183       EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
 184     }
 185   }
 186 }
 187
 188 TEST(Grid2D, TwoDataDimensionColMajorStacked) {
 189   int x[] = {1,   2,
 190              2,   3,
 191              3,   4,
 192              4,   8,
 193              8,  12,
 194              12, 16};
 195   Grid2D<int, 2, false, false> grid(x, 0, 2, 0, 3);
 196   for (int r = 0; r < 2; ++r) {
 197     for (int c = 0; c < 3; ++c) {
 198       double value[2];
 199       grid.GetValue(r, c, value);
 200       EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
 201       EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
 202     }
 203   }
 204 }
 205
 206 class CubicInterpolatorTest : public ::testing::Test {
 207  public:
 208   template <int kDataDimension>
 209   void RunPolynomialInterpolationTest(const double a,
 210                                       const double b,
 211                                       const double c,
 212                                       const double d) {
 213     values_.reset(new double[kDataDimension * kNumSamples]);
 214
 215     for (int x = 0; x < kNumSamples; ++x) {
 216       for (int dim = 0; dim < kDataDimension; ++dim) {
 217       values_[x * kDataDimension + dim] =
 218           (dim * dim  + 1) * (a  * x * x * x + b * x * x + c * x + d);
 219       }
 220     }
 221
 222     Grid1D<double, kDataDimension> grid(values_.get(), 0, kNumSamples);
 223     CubicInterpolator<Grid1D<double, kDataDimension> > interpolator(grid);
 224
 225     // Check values in the all the cells but the first and the last
 226     // ones. In these cells, the interpolated function values should
 227     // match exactly the values of the function being interpolated.
 228     //
 229     // On the boundary, we extrapolate the values of the function on
 230     // the basis of its first derivative, so we do not expect the
 231     // function values and its derivatives not to match.
 232     for (int j = 0; j < kNumTestSamples; ++j) {
 233       const double x = 1.0 + 7.0 / (kNumTestSamples - 1) * j;
 234       double expected_f[kDataDimension], expected_dfdx[kDataDimension];
 235       double f[kDataDimension], dfdx[kDataDimension];
 236
 237       for (int dim = 0; dim < kDataDimension; ++dim) {
 238         expected_f[dim] =
 239             (dim * dim  + 1) * (a  * x * x * x + b * x * x + c * x + d);
 240         expected_dfdx[dim] = (dim * dim + 1) * (3.0 * a * x * x + 2.0 * b * x + c);
 241       }
 242
 243       interpolator.Evaluate(x, f, dfdx);
 244       for (int dim = 0; dim < kDataDimension; ++dim) {
 245         EXPECT_NEAR(f[dim], expected_f[dim], kTolerance)
 246             << "x: " << x << " dim: " << dim
 247             << " actual f(x): " << expected_f[dim]
 248             << " estimated f(x): " << f[dim];
 249         EXPECT_NEAR(dfdx[dim], expected_dfdx[dim], kTolerance)
 250             << "x: " << x << " dim: " << dim
 251             << " actual df(x)/dx: " << expected_dfdx[dim]
 252             << " estimated df(x)/dx: " << dfdx[dim];
 253       }
 254     }
 255   }
 256
 257  private:
 258   static const int kNumSamples = 10;
 259   static const int kNumTestSamples = 100;
 260   scoped_array<double> values_;
 261 };
 262
 263 TEST_F(CubicInterpolatorTest, ConstantFunction) {
 264   RunPolynomialInterpolationTest<1>(0.0, 0.0, 0.0, 0.5);
 265   RunPolynomialInterpolationTest<2>(0.0, 0.0, 0.0, 0.5);
 266   RunPolynomialInterpolationTest<3>(0.0, 0.0, 0.0, 0.5);
 267 }
 268
 269 TEST_F(CubicInterpolatorTest, LinearFunction) {
 270   RunPolynomialInterpolationTest<1>(0.0, 0.0, 1.0, 0.5);
 271   RunPolynomialInterpolationTest<2>(0.0, 0.0, 1.0, 0.5);
 272   RunPolynomialInterpolationTest<3>(0.0, 0.0, 1.0, 0.5);
 273 }
 274
 275 TEST_F(CubicInterpolatorTest, QuadraticFunction) {
 276   RunPolynomialInterpolationTest<1>(0.0, 0.4, 1.0, 0.5);
 277   RunPolynomialInterpolationTest<2>(0.0, 0.4, 1.0, 0.5);
 278   RunPolynomialInterpolationTest<3>(0.0, 0.4, 1.0, 0.5);
 279 }
 280
 281
 282 TEST(CubicInterpolator, JetEvaluation) {
 283   const double values[] = {1.0, 2.0, 2.0, 5.0, 3.0, 9.0, 2.0, 7.0};
 284
 285   Grid1D<double, 2, true> grid(values, 0, 4);
 286   CubicInterpolator<Grid1D<double, 2, true> > interpolator(grid);
 287
 288   double f[2], dfdx[2];
 289   const double x = 2.5;
 290   interpolator.Evaluate(x, f, dfdx);
 291
 292   // Create a Jet with the same scalar part as x, so that the output
 293   // Jet will be evaluated at x.
 294   Jet<double, 4> x_jet;
 295   x_jet.a = x;
 296   x_jet.v(0) = 1.0;
 297   x_jet.v(1) = 1.1;
 298   x_jet.v(2) = 1.2;
 299   x_jet.v(3) = 1.3;
 300
 301   Jet<double, 4> f_jets[2];
 302   interpolator.Evaluate(x_jet, f_jets);
 303
 304   // Check that the scalar part of the Jet is f(x).
 305   EXPECT_EQ(f_jets[0].a, f[0]);
 306   EXPECT_EQ(f_jets[1].a, f[1]);
 307
 308   // Check that the derivative part of the Jet is dfdx * x_jet.v
 309   // by the chain rule.
 310   EXPECT_NEAR((f_jets[0].v - dfdx[0] * x_jet.v).norm(), 0.0, kTolerance);
 311   EXPECT_NEAR((f_jets[1].v - dfdx[1] * x_jet.v).norm(), 0.0, kTolerance);
 312 }
 313
 314 class BiCubicInterpolatorTest : public ::testing::Test {
 315  public:
 316   template <int kDataDimension>
 317   void RunPolynomialInterpolationTest(const Eigen::Matrix3d& coeff) {
 318     values_.reset(new double[kNumRows * kNumCols * kDataDimension]);
 319     coeff_ = coeff;
 320     double* v = values_.get();
 321     for (int r = 0; r < kNumRows; ++r) {
 322       for (int c = 0; c < kNumCols; ++c) {
 323         for (int dim = 0; dim < kDataDimension; ++dim) {
 324           *v++ = (dim * dim + 1) * EvaluateF(r, c);
 325         }
 326       }
 327     }
 328
 329     Grid2D<double, kDataDimension> grid(values_.get(), 0, kNumRows, 0, kNumCols);
 330     BiCubicInterpolator<Grid2D<double, kDataDimension> > interpolator(grid);
 331
 332     for (int j = 0; j < kNumRowSamples; ++j) {
 333       const double r = 1.0 + 7.0 / (kNumRowSamples - 1) * j;
 334       for (int k = 0; k < kNumColSamples; ++k) {
 335         const double c = 1.0 + 7.0 / (kNumColSamples - 1) * k;
 336         double f[kDataDimension], dfdr[kDataDimension], dfdc[kDataDimension];
 337         interpolator.Evaluate(r, c, f, dfdr, dfdc);
 338         for (int dim = 0; dim < kDataDimension; ++dim) {
 339           EXPECT_NEAR(f[dim], (dim * dim + 1) * EvaluateF(r, c), kTolerance);
 340           EXPECT_NEAR(dfdr[dim], (dim * dim + 1) * EvaluatedFdr(r, c), kTolerance);
 341           EXPECT_NEAR(dfdc[dim], (dim * dim + 1) * EvaluatedFdc(r, c), kTolerance);
 342         }
 343       }
 344     }
 345   }
 346
 347  private:
 348   double EvaluateF(double r, double c) {
 349     Eigen::Vector3d x;
 350     x(0) = r;
 351     x(1) = c;
 352     x(2) = 1;
 353     return x.transpose() * coeff_ * x;
 354   }
 355
 356   double EvaluatedFdr(double r, double c) {
 357     Eigen::Vector3d x;
 358     x(0) = r;
 359     x(1) = c;
 360     x(2) = 1;
 361     return (coeff_.row(0) + coeff_.col(0).transpose()) * x;
 362   }
 363
 364   double EvaluatedFdc(double r, double c) {
 365     Eigen::Vector3d x;
 366     x(0) = r;
 367     x(1) = c;
 368     x(2) = 1;
 369     return (coeff_.row(1) + coeff_.col(1).transpose()) * x;
 370   }
 371
 372
 373   Eigen::Matrix3d coeff_;
 374   static const int kNumRows = 10;
 375   static const int kNumCols = 10;
 376   static const int kNumRowSamples = 100;
 377   static const int kNumColSamples = 100;
 378   scoped_array<double> values_;
 379 };
 380
 381 TEST_F(BiCubicInterpolatorTest, ZeroFunction) {
 382   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 383   RunPolynomialInterpolationTest<1>(coeff);
 384   RunPolynomialInterpolationTest<2>(coeff);
 385   RunPolynomialInterpolationTest<3>(coeff);
 386 }
 387
 388 TEST_F(BiCubicInterpolatorTest, Degree00Function) {
 389   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 390   coeff(2, 2) = 1.0;
 391   RunPolynomialInterpolationTest<1>(coeff);
 392   RunPolynomialInterpolationTest<2>(coeff);
 393   RunPolynomialInterpolationTest<3>(coeff);
 394 }
 395
 396 TEST_F(BiCubicInterpolatorTest, Degree01Function) {
 397   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 398   coeff(2, 2) = 1.0;
 399   coeff(0, 2) = 0.1;
 400   coeff(2, 0) = 0.1;
 401   RunPolynomialInterpolationTest<1>(coeff);
 402   RunPolynomialInterpolationTest<2>(coeff);
 403   RunPolynomialInterpolationTest<3>(coeff);
 404 }
 405
 406 TEST_F(BiCubicInterpolatorTest, Degree10Function) {
 407   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 408   coeff(2, 2) = 1.0;
 409   coeff(0, 1) = 0.1;
 410   coeff(1, 0) = 0.1;
 411   RunPolynomialInterpolationTest<1>(coeff);
 412   RunPolynomialInterpolationTest<2>(coeff);
 413   RunPolynomialInterpolationTest<3>(coeff);
 414 }
 415
 416 TEST_F(BiCubicInterpolatorTest, Degree11Function) {
 417   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 418   coeff(2, 2) = 1.0;
 419   coeff(0, 1) = 0.1;
 420   coeff(1, 0) = 0.1;
 421   coeff(0, 2) = 0.2;
 422   coeff(2, 0) = 0.2;
 423   RunPolynomialInterpolationTest<1>(coeff);
 424   RunPolynomialInterpolationTest<2>(coeff);
 425   RunPolynomialInterpolationTest<3>(coeff);
 426 }
 427
 428 TEST_F(BiCubicInterpolatorTest, Degree12Function) {
 429   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 430   coeff(2, 2) = 1.0;
 431   coeff(0, 1) = 0.1;
 432   coeff(1, 0) = 0.1;
 433   coeff(0, 2) = 0.2;
 434   coeff(2, 0) = 0.2;
 435   coeff(1, 1) = 0.3;
 436   RunPolynomialInterpolationTest<1>(coeff);
 437   RunPolynomialInterpolationTest<2>(coeff);
 438   RunPolynomialInterpolationTest<3>(coeff);
 439 }
 440
 441 TEST_F(BiCubicInterpolatorTest, Degree21Function) {
 442   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 443   coeff(2, 2) = 1.0;
 444   coeff(0, 1) = 0.1;
 445   coeff(1, 0) = 0.1;
 446   coeff(0, 2) = 0.2;
 447   coeff(2, 0) = 0.2;
 448   coeff(0, 0) = 0.3;
 449   RunPolynomialInterpolationTest<1>(coeff);
 450   RunPolynomialInterpolationTest<2>(coeff);
 451   RunPolynomialInterpolationTest<3>(coeff);
 452 }
 453
 454 TEST_F(BiCubicInterpolatorTest, Degree22Function) {
 455   Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
 456   coeff(2, 2) = 1.0;
 457   coeff(0, 1) = 0.1;
 458   coeff(1, 0) = 0.1;
 459   coeff(0, 2) = 0.2;
 460   coeff(2, 0) = 0.2;
 461   coeff(0, 0) = 0.3;
 462   coeff(0, 1) = -0.4;
 463   coeff(1, 0) = -0.4;
 464   RunPolynomialInterpolationTest<1>(coeff);
 465   RunPolynomialInterpolationTest<2>(coeff);
 466   RunPolynomialInterpolationTest<3>(coeff);
 467 }
 468
 469 TEST(BiCubicInterpolator, JetEvaluation) {
 470   const double values[] = {1.0, 5.0, 2.0, 10.0, 2.0, 6.0, 3.0, 5.0,
 471                            1.0, 2.0, 2.0,  2.0, 2.0, 2.0, 3.0, 1.0};
 472
 473   Grid2D<double, 2> grid(values, 0, 2, 0, 4);
 474   BiCubicInterpolator<Grid2D<double, 2> > interpolator(grid);
 475
 476   double f[2], dfdr[2], dfdc[2];
 477   const double r = 0.5;
 478   const double c = 2.5;
 479   interpolator.Evaluate(r, c, f, dfdr, dfdc);
 480
 481   // Create a Jet with the same scalar part as x, so that the output
 482   // Jet will be evaluated at x.
 483   Jet<double, 4> r_jet;
 484   r_jet.a = r;
 485   r_jet.v(0) = 1.0;
 486   r_jet.v(1) = 1.1;
 487   r_jet.v(2) = 1.2;
 488   r_jet.v(3) = 1.3;
 489
 490   Jet<double, 4> c_jet;
 491   c_jet.a = c;
 492   c_jet.v(0) = 2.0;
 493   c_jet.v(1) = 3.1;
 494   c_jet.v(2) = 4.2;
 495   c_jet.v(3) = 5.3;
 496
 497   Jet<double, 4> f_jets[2];
 498   interpolator.Evaluate(r_jet, c_jet, f_jets);
 499   EXPECT_EQ(f_jets[0].a, f[0]);
 500   EXPECT_EQ(f_jets[1].a, f[1]);
 501   EXPECT_NEAR((f_jets[0].v - dfdr[0] * r_jet.v - dfdc[0] * c_jet.v).norm(),
 502               0.0,
 503               kTolerance);
 504   EXPECT_NEAR((f_jets[1].v - dfdr[1] * r_jet.v - dfdc[1] * c_jet.v).norm(),
 505               0.0,
 506               kTolerance);
 507 }
 508
 509 }  // namespace internal
 510 }  // namespace ceres