10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: keir@google.com (Keir Mierle) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Utility functions useful for testing. 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include <algorithm> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cmath> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/file.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stringprintf.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gflags/gflags.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongDECLARE_string(test_srcdir); 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// This macro is used to inject additional path information specific 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to the build system. 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_TEST_SRCDIR_SUFFIX 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_TEST_SRCDIR_SUFFIX "" 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool ExpectClose(double x, double y, double max_abs_relative_difference) { 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double absolute_difference = fabs(x - y); 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double relative_difference = absolute_difference / std::max(fabs(x), fabs(y)); 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (x == 0 || y == 0) { 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If x or y is exactly zero, then relative difference doesn't have any 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // meaning. Take the absolute difference instead. 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong relative_difference = absolute_difference; 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (relative_difference > max_abs_relative_difference) { 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(1) << StringPrintf("x=%17g y=%17g abs=%17g rel=%17g", 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x, y, absolute_difference, relative_difference); 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(relative_difference, 0.0, max_abs_relative_difference); 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return relative_difference <= max_abs_relative_difference; 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ExpectArraysCloseUptoScale(int n, 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* p, 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* q, 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double tol) { 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GT(n, 0); 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(p); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(q); 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double p_max = 0; 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double q_max = 0; 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int p_i = 0; 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int q_i = 0; 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < n; ++i) { 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (std::abs(p[i]) > p_max) { 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong p_max = std::abs(p[i]); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong p_i = i; 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (std::abs(q[i]) > q_max) { 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong q_max = std::abs(q[i]); 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong q_i = i; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If both arrays are all zeros, they are equal up to scale, but 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // for testing purposes, that's more likely to be an error than 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // a desired result. 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NE(p_max, 0.0); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NE(q_max, 0.0); 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < n; ++i) { 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double p_norm = p[i] / p[p_i]; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double q_norm = q[i] / q[q_i]; 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(p_norm, q_norm, tol) << "i=" << i; 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ExpectArraysClose(int n, 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* p, 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* q, 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double tol) { 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GT(n, 0); 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(p); 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(q); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < n; ++i) { 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(p[i], q[i], tol) << "i=" << i; 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstring TestFileAbsolutePath(const string& filename) { 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return JoinPath(FLAGS_test_srcdir + CERES_TEST_SRCDIR_SUFFIX, 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong filename); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 129