1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 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: moll.markus (at) arcor.de (Markus Moll) 30 // sameeragarwal (at) google.com (Sameer Agarwal) 31 32 #include "ceres/polynomial.h" 33 34 #include <limits> 35 #include <cmath> 36 #include <cstddef> 37 #include <algorithm> 38 #include "gtest/gtest.h" 39 #include "ceres/test_util.h" 40 41 namespace ceres { 42 namespace internal { 43 namespace { 44 45 // For IEEE-754 doubles, machine precision is about 2e-16. 46 const double kEpsilon = 1e-13; 47 const double kEpsilonLoose = 1e-9; 48 49 // Return the constant polynomial p(x) = 1.23. 50 Vector ConstantPolynomial(double value) { 51 Vector poly(1); 52 poly(0) = value; 53 return poly; 54 } 55 56 // Return the polynomial p(x) = poly(x) * (x - root). 57 Vector AddRealRoot(const Vector& poly, double root) { 58 Vector poly2(poly.size() + 1); 59 poly2.setZero(); 60 poly2.head(poly.size()) += poly; 61 poly2.tail(poly.size()) -= root * poly; 62 return poly2; 63 } 64 65 // Return the polynomial 66 // p(x) = poly(x) * (x - real - imag*i) * (x - real + imag*i). 67 Vector AddComplexRootPair(const Vector& poly, double real, double imag) { 68 Vector poly2(poly.size() + 2); 69 poly2.setZero(); 70 // Multiply poly by x^2 - 2real + abs(real,imag)^2 71 poly2.head(poly.size()) += poly; 72 poly2.segment(1, poly.size()) -= 2 * real * poly; 73 poly2.tail(poly.size()) += (real*real + imag*imag) * poly; 74 return poly2; 75 } 76 77 // Sort the entries in a vector. 78 // Needed because the roots are not returned in sorted order. 79 Vector SortVector(const Vector& in) { 80 Vector out(in); 81 std::sort(out.data(), out.data() + out.size()); 82 return out; 83 } 84 85 // Run a test with the polynomial defined by the N real roots in roots_real. 86 // If use_real is false, NULL is passed as the real argument to 87 // FindPolynomialRoots. If use_imaginary is false, NULL is passed as the 88 // imaginary argument to FindPolynomialRoots. 89 template<int N> 90 void RunPolynomialTestRealRoots(const double (&real_roots)[N], 91 bool use_real, 92 bool use_imaginary, 93 double epsilon) { 94 Vector real; 95 Vector imaginary; 96 Vector poly = ConstantPolynomial(1.23); 97 for (int i = 0; i < N; ++i) { 98 poly = AddRealRoot(poly, real_roots[i]); 99 } 100 Vector* const real_ptr = use_real ? &real : NULL; 101 Vector* const imaginary_ptr = use_imaginary ? &imaginary : NULL; 102 bool success = FindPolynomialRoots(poly, real_ptr, imaginary_ptr); 103 104 EXPECT_EQ(success, true); 105 if (use_real) { 106 EXPECT_EQ(real.size(), N); 107 real = SortVector(real); 108 ExpectArraysClose(N, real.data(), real_roots, epsilon); 109 } 110 if (use_imaginary) { 111 EXPECT_EQ(imaginary.size(), N); 112 const Vector zeros = Vector::Zero(N); 113 ExpectArraysClose(N, imaginary.data(), zeros.data(), epsilon); 114 } 115 } 116 } // namespace 117 118 TEST(Polynomial, InvalidPolynomialOfZeroLengthIsRejected) { 119 // Vector poly(0) is an ambiguous constructor call, so 120 // use the constructor with explicit column count. 121 Vector poly(0, 1); 122 Vector real; 123 Vector imag; 124 bool success = FindPolynomialRoots(poly, &real, &imag); 125 126 EXPECT_EQ(success, false); 127 } 128 129 TEST(Polynomial, ConstantPolynomialReturnsNoRoots) { 130 Vector poly = ConstantPolynomial(1.23); 131 Vector real; 132 Vector imag; 133 bool success = FindPolynomialRoots(poly, &real, &imag); 134 135 EXPECT_EQ(success, true); 136 EXPECT_EQ(real.size(), 0); 137 EXPECT_EQ(imag.size(), 0); 138 } 139 140 TEST(Polynomial, LinearPolynomialWithPositiveRootWorks) { 141 const double roots[1] = { 42.42 }; 142 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 143 } 144 145 TEST(Polynomial, LinearPolynomialWithNegativeRootWorks) { 146 const double roots[1] = { -42.42 }; 147 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 148 } 149 150 TEST(Polynomial, QuadraticPolynomialWithPositiveRootsWorks) { 151 const double roots[2] = { 1.0, 42.42 }; 152 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 153 } 154 155 TEST(Polynomial, QuadraticPolynomialWithOneNegativeRootWorks) { 156 const double roots[2] = { -42.42, 1.0 }; 157 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 158 } 159 160 TEST(Polynomial, QuadraticPolynomialWithTwoNegativeRootsWorks) { 161 const double roots[2] = { -42.42, -1.0 }; 162 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 163 } 164 165 TEST(Polynomial, QuadraticPolynomialWithCloseRootsWorks) { 166 const double roots[2] = { 42.42, 42.43 }; 167 RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose); 168 } 169 170 TEST(Polynomial, QuadraticPolynomialWithComplexRootsWorks) { 171 Vector real; 172 Vector imag; 173 174 Vector poly = ConstantPolynomial(1.23); 175 poly = AddComplexRootPair(poly, 42.42, 4.2); 176 bool success = FindPolynomialRoots(poly, &real, &imag); 177 178 EXPECT_EQ(success, true); 179 EXPECT_EQ(real.size(), 2); 180 EXPECT_EQ(imag.size(), 2); 181 ExpectClose(real(0), 42.42, kEpsilon); 182 ExpectClose(real(1), 42.42, kEpsilon); 183 ExpectClose(std::abs(imag(0)), 4.2, kEpsilon); 184 ExpectClose(std::abs(imag(1)), 4.2, kEpsilon); 185 ExpectClose(std::abs(imag(0) + imag(1)), 0.0, kEpsilon); 186 } 187 188 TEST(Polynomial, QuarticPolynomialWorks) { 189 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 190 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 191 } 192 193 TEST(Polynomial, QuarticPolynomialWithTwoClustersOfCloseRootsWorks) { 194 const double roots[4] = { 1.23e-1, 2.46e-1, 1.23e+5, 2.46e+5 }; 195 RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); 196 } 197 198 TEST(Polynomial, QuarticPolynomialWithTwoZeroRootsWorks) { 199 const double roots[4] = { -42.42, 0.0, 0.0, 42.42 }; 200 RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); 201 } 202 203 TEST(Polynomial, QuarticMonomialWorks) { 204 const double roots[4] = { 0.0, 0.0, 0.0, 0.0 }; 205 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 206 } 207 208 TEST(Polynomial, NullPointerAsImaginaryPartWorks) { 209 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 210 RunPolynomialTestRealRoots(roots, true, false, kEpsilon); 211 } 212 213 TEST(Polynomial, NullPointerAsRealPartWorks) { 214 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 215 RunPolynomialTestRealRoots(roots, false, true, kEpsilon); 216 } 217 218 TEST(Polynomial, BothOutputArgumentsNullWorks) { 219 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 220 RunPolynomialTestRealRoots(roots, false, false, kEpsilon); 221 } 222 223 TEST(Polynomial, DifferentiateConstantPolynomial) { 224 // p(x) = 1; 225 Vector polynomial(1); 226 polynomial(0) = 1.0; 227 const Vector derivative = DifferentiatePolynomial(polynomial); 228 EXPECT_EQ(derivative.rows(), 1); 229 EXPECT_EQ(derivative(0), 0); 230 } 231 232 TEST(Polynomial, DifferentiateQuadraticPolynomial) { 233 // p(x) = x^2 + 2x + 3; 234 Vector polynomial(3); 235 polynomial(0) = 1.0; 236 polynomial(1) = 2.0; 237 polynomial(2) = 3.0; 238 239 const Vector derivative = DifferentiatePolynomial(polynomial); 240 EXPECT_EQ(derivative.rows(), 2); 241 EXPECT_EQ(derivative(0), 2.0); 242 EXPECT_EQ(derivative(1), 2.0); 243 } 244 245 TEST(Polynomial, MinimizeConstantPolynomial) { 246 // p(x) = 1; 247 Vector polynomial(1); 248 polynomial(0) = 1.0; 249 250 double optimal_x = 0.0; 251 double optimal_value = 0.0; 252 double min_x = 0.0; 253 double max_x = 1.0; 254 MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); 255 256 EXPECT_EQ(optimal_value, 1.0); 257 EXPECT_LE(optimal_x, max_x); 258 EXPECT_GE(optimal_x, min_x); 259 } 260 261 TEST(Polynomial, MinimizeLinearPolynomial) { 262 // p(x) = x - 2 263 Vector polynomial(2); 264 265 polynomial(0) = 1.0; 266 polynomial(1) = 2.0; 267 268 double optimal_x = 0.0; 269 double optimal_value = 0.0; 270 double min_x = 0.0; 271 double max_x = 1.0; 272 MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); 273 274 EXPECT_EQ(optimal_x, 0.0); 275 EXPECT_EQ(optimal_value, 2.0); 276 } 277 278 279 TEST(Polynomial, MinimizeQuadraticPolynomial) { 280 // p(x) = x^2 - 3 x + 2 281 // min_x = 3/2 282 // min_value = -1/4; 283 Vector polynomial(3); 284 polynomial(0) = 1.0; 285 polynomial(1) = -3.0; 286 polynomial(2) = 2.0; 287 288 double optimal_x = 0.0; 289 double optimal_value = 0.0; 290 double min_x = -2.0; 291 double max_x = 2.0; 292 MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); 293 EXPECT_EQ(optimal_x, 3.0/2.0); 294 EXPECT_EQ(optimal_value, -1.0/4.0); 295 296 min_x = -2.0; 297 max_x = 1.0; 298 MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); 299 EXPECT_EQ(optimal_x, 1.0); 300 EXPECT_EQ(optimal_value, 0.0); 301 302 min_x = 2.0; 303 max_x = 3.0; 304 MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); 305 EXPECT_EQ(optimal_x, 2.0); 306 EXPECT_EQ(optimal_value, 0.0); 307 } 308 309 TEST(Polymomial, ConstantInterpolatingPolynomial) { 310 // p(x) = 1.0 311 Vector true_polynomial(1); 312 true_polynomial << 1.0; 313 314 vector<FunctionSample> samples; 315 FunctionSample sample; 316 sample.x = 1.0; 317 sample.value = 1.0; 318 sample.value_is_valid = true; 319 samples.push_back(sample); 320 321 const Vector polynomial = FindInterpolatingPolynomial(samples); 322 EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15); 323 } 324 325 TEST(Polynomial, LinearInterpolatingPolynomial) { 326 // p(x) = 2x - 1 327 Vector true_polynomial(2); 328 true_polynomial << 2.0, -1.0; 329 330 vector<FunctionSample> samples; 331 FunctionSample sample; 332 sample.x = 1.0; 333 sample.value = 1.0; 334 sample.value_is_valid = true; 335 sample.gradient = 2.0; 336 sample.gradient_is_valid = true; 337 samples.push_back(sample); 338 339 const Vector polynomial = FindInterpolatingPolynomial(samples); 340 EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15); 341 } 342 343 TEST(Polynomial, QuadraticInterpolatingPolynomial) { 344 // p(x) = 2x^2 + 3x + 2 345 Vector true_polynomial(3); 346 true_polynomial << 2.0, 3.0, 2.0; 347 348 vector<FunctionSample> samples; 349 { 350 FunctionSample sample; 351 sample.x = 1.0; 352 sample.value = 7.0; 353 sample.value_is_valid = true; 354 sample.gradient = 7.0; 355 sample.gradient_is_valid = true; 356 samples.push_back(sample); 357 } 358 359 { 360 FunctionSample sample; 361 sample.x = -3.0; 362 sample.value = 11.0; 363 sample.value_is_valid = true; 364 samples.push_back(sample); 365 } 366 367 Vector polynomial = FindInterpolatingPolynomial(samples); 368 EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15); 369 } 370 371 TEST(Polynomial, DeficientCubicInterpolatingPolynomial) { 372 // p(x) = 2x^2 + 3x + 2 373 Vector true_polynomial(4); 374 true_polynomial << 0.0, 2.0, 3.0, 2.0; 375 376 vector<FunctionSample> samples; 377 { 378 FunctionSample sample; 379 sample.x = 1.0; 380 sample.value = 7.0; 381 sample.value_is_valid = true; 382 sample.gradient = 7.0; 383 sample.gradient_is_valid = true; 384 samples.push_back(sample); 385 } 386 387 { 388 FunctionSample sample; 389 sample.x = -3.0; 390 sample.value = 11.0; 391 sample.value_is_valid = true; 392 sample.gradient = -9; 393 sample.gradient_is_valid = true; 394 samples.push_back(sample); 395 } 396 397 const Vector polynomial = FindInterpolatingPolynomial(samples); 398 EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); 399 } 400 401 402 TEST(Polynomial, CubicInterpolatingPolynomialFromValues) { 403 // p(x) = x^3 + 2x^2 + 3x + 2 404 Vector true_polynomial(4); 405 true_polynomial << 1.0, 2.0, 3.0, 2.0; 406 407 vector<FunctionSample> samples; 408 { 409 FunctionSample sample; 410 sample.x = 1.0; 411 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 412 sample.value_is_valid = true; 413 samples.push_back(sample); 414 } 415 416 { 417 FunctionSample sample; 418 sample.x = -3.0; 419 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 420 sample.value_is_valid = true; 421 samples.push_back(sample); 422 } 423 424 { 425 FunctionSample sample; 426 sample.x = 2.0; 427 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 428 sample.value_is_valid = true; 429 samples.push_back(sample); 430 } 431 432 { 433 FunctionSample sample; 434 sample.x = 0.0; 435 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 436 sample.value_is_valid = true; 437 samples.push_back(sample); 438 } 439 440 const Vector polynomial = FindInterpolatingPolynomial(samples); 441 EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); 442 } 443 444 TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndOneGradient) { 445 // p(x) = x^3 + 2x^2 + 3x + 2 446 Vector true_polynomial(4); 447 true_polynomial << 1.0, 2.0, 3.0, 2.0; 448 Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial); 449 450 vector<FunctionSample> samples; 451 { 452 FunctionSample sample; 453 sample.x = 1.0; 454 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 455 sample.value_is_valid = true; 456 samples.push_back(sample); 457 } 458 459 { 460 FunctionSample sample; 461 sample.x = -3.0; 462 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 463 sample.value_is_valid = true; 464 samples.push_back(sample); 465 } 466 467 { 468 FunctionSample sample; 469 sample.x = 2.0; 470 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 471 sample.value_is_valid = true; 472 sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x); 473 sample.gradient_is_valid = true; 474 samples.push_back(sample); 475 } 476 477 const Vector polynomial = FindInterpolatingPolynomial(samples); 478 EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); 479 } 480 481 TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndGradients) { 482 // p(x) = x^3 + 2x^2 + 3x + 2 483 Vector true_polynomial(4); 484 true_polynomial << 1.0, 2.0, 3.0, 2.0; 485 Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial); 486 487 vector<FunctionSample> samples; 488 { 489 FunctionSample sample; 490 sample.x = -3.0; 491 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 492 sample.value_is_valid = true; 493 sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x); 494 sample.gradient_is_valid = true; 495 samples.push_back(sample); 496 } 497 498 { 499 FunctionSample sample; 500 sample.x = 2.0; 501 sample.value = EvaluatePolynomial(true_polynomial, sample.x); 502 sample.value_is_valid = true; 503 sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x); 504 sample.gradient_is_valid = true; 505 samples.push_back(sample); 506 } 507 508 const Vector polynomial = FindInterpolatingPolynomial(samples); 509 EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); 510 } 511 512 } // namespace internal 513 } // namespace ceres 514
