internal/ceres/system_test.cc - ceres-solver - Git at Google


            
            // Ceres Solver - A fast non-linear least squares minimizer

            
            // Copyright 2010, 2011, 2012 Google Inc. All rights reserved.

            
            // http://code.google.com/p/ceres-solver/

            
            //

            
            // 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: keir@google.com (Keir Mierle)

            
            //         sameeragarwal@google.com (Sameer Agarwal)

            
            //

            
            // System level tests for Ceres. The current suite of two tests. The

            
            // first test is a small test based on Powell's Function. It is a

            
            // scalar problem with 4 variables. The second problem is a bundle

            
            // adjustment problem with 16 cameras and two thousand cameras. The

            
            // first problem is to test the sanity test the factorization based

            
            // solvers. The second problem is used to test the various

            
            // combinations of solvers, orderings, preconditioners and

            
            // multithreading.


            #include <cmath>

            
            #include <cstdio>

            
            #include <cstdlib>

            
            #include <string>


            #include <glog/logging.h>

            
            #include "ceres/file.h"

            
            #include "gflags/gflags.h"

            
            #include "gtest/gtest.h"

            
            #include "ceres/stringprintf.h"

            
            #include "ceres/test_util.h"

            
            #include "ceres/autodiff_cost_function.h"

            
            #include "ceres/problem.h"

            
            #include "ceres/solver.h"

            
            #include "ceres/types.h"

            
            #include "ceres/rotation.h"


            DECLARE_string(test_srcdir);


            namespace ceres {

            
            namespace internal {


            // Struct used for configuring the solver.

            
            struct SolverConfig {

            
              SolverConfig(LinearSolverType linear_solver_type,

            
                           SparseLinearAlgebraLibraryType sparse_linear_algebra_library,

            
                           OrderingType ordering_type)

            
                  : linear_solver_type(linear_solver_type),

            
                    sparse_linear_algebra_library(sparse_linear_algebra_library),

            
                    ordering_type(ordering_type),

            
                    preconditioner_type(IDENTITY),

            
                    num_threads(1) {

            
              }


              SolverConfig(LinearSolverType linear_solver_type,

            
                           SparseLinearAlgebraLibraryType sparse_linear_algebra_library,

            
                           OrderingType ordering_type,

            
                           PreconditionerType preconditioner_type,

            
                           int num_threads)

            
                  : linear_solver_type(linear_solver_type),

            
                    sparse_linear_algebra_library(sparse_linear_algebra_library),

            
                    ordering_type(ordering_type),

            
                    preconditioner_type(preconditioner_type),

            
                    num_threads(num_threads) {

            
              }


              string ToString() const {

            
                return StringPrintf(

            
                    "(%s, %s, %s, %s, %d)",

            
                    LinearSolverTypeToString(linear_solver_type),

            
                    SparseLinearAlgebraLibraryTypeToString(sparse_linear_algebra_library),

            
                    OrderingTypeToString(ordering_type),

            
                    PreconditionerTypeToString(preconditioner_type),

            
                    num_threads);

            
              }


              LinearSolverType linear_solver_type;

            
              SparseLinearAlgebraLibraryType sparse_linear_algebra_library;

            
              OrderingType ordering_type;

            
              PreconditionerType preconditioner_type;

            
              int num_threads;

            
            };


            // Templated function that given a set of solver configurations,

            
            // instantiates a new copy of SystemTestProblem for each configuration

            
            // and solves it. The solutions are expected to have residuals with

            
            // coordinate-wise maximum absolute difference less than or equal to

            
            // max_abs_difference.

            
            //

            
            // The template parameter SystemTestProblem is expected to implement

            
            // the following interface.

            
            //

            
            //   class SystemTestProblem {

            
            //     public:

            
            //       SystemTestProblem();

            
            //       Problem* mutable_problem();

            
            //       Solver::Options* mutable_solver_options();

            
            //   };

            
            template <typename SystemTestProblem>

            
            void RunSolversAndCheckTheyMatch(const vector<SolverConfig>& configurations,

            
                                             const double max_abs_difference) {

            
              int num_configurations = configurations.size();

            
              vector<SystemTestProblem*> problems;

            
              vector<Solver::Summary> summaries(num_configurations);


              for (int i = 0; i < num_configurations; ++i) {

            
                SystemTestProblem* system_test_problem = new SystemTestProblem();


                const SolverConfig& config = configurations[i];


                Solver::Options& options = *(system_test_problem->mutable_solver_options());

            
                options.linear_solver_type = config.linear_solver_type;

            
                options.sparse_linear_algebra_library =

            
                    config.sparse_linear_algebra_library;

            
                options.ordering_type = config.ordering_type;

            
                options.preconditioner_type = config.preconditioner_type;

            
                options.num_threads = config.num_threads;

            
                options.num_linear_solver_threads = config.num_threads;

            
                options.return_final_residuals = true;


                if (options.ordering_type == SCHUR || options.ordering_type == NATURAL) {

            
                  options.ordering.clear();

            
                }


                if (options.ordering_type == SCHUR) {

            
                  options.num_eliminate_blocks = 0;

            
                }


                LOG(INFO) << "Running solver configuration: "

            
                          << config.ToString();


                Solve(options,

            
                      system_test_problem->mutable_problem(),

            
                      &summaries[i]);


                CHECK_NE(summaries[i].termination_type, ceres::NUMERICAL_FAILURE)

            
                    << "Solver configuration " << i << " failed.";

            
                problems.push_back(system_test_problem);


                // Compare the resulting solutions to each other. Arbitrarily take

            
                // SPARSE_NORMAL_CHOLESKY as the golden solve. We compare

            
                // solutions by comparing their residual vectors. We do not

            
                // compare parameter vectors because it is much more brittle and

            
                // error prone to do so, since the same problem can have nearly

            
                // the same residuals at two completely different positions in

            
                // parameter space.

            
                if (i > 0) {

            
                  const vector<double>& reference_residuals = summaries[0].final_residuals;

            
                  const vector<double>& current_residuals = summaries[i].final_residuals;


                  for (int j = 0; j < reference_residuals.size(); ++j) {

            
                    EXPECT_NEAR(current_residuals[j],

            
                                reference_residuals[j],

            
                                max_abs_difference)

            
                        << "Not close enough residual:" << j

            
                        << " reference " << reference_residuals[j]

            
                        << " current " << current_residuals[j];

            
                  }

            
                }

            
              }


              for (int i = 0; i < num_configurations; ++i) {

            
                delete problems[i];

            
              }

            
            }


            // This class implements the SystemTestProblem interface and provides

            
            // access to an implementation of Powell's singular function.

            
            //

            
            //   F = 1/2 (f1^2 + f2^2 + f3^2 + f4^2)

            
            //

            
            //   f1 = x1 + 10*x2;

            
            //   f2 = sqrt(5) * (x3 - x4)

            
            //   f3 = (x2 - 2*x3)^2

            
            //   f4 = sqrt(10) * (x1 - x4)^2

            
            //

            
            // The starting values are x1 = 3, x2 = -1, x3 = 0, x4 = 1.

            
            // The minimum is 0 at (x1, x2, x3, x4) = 0.

            
            //

            
            // From: Testing Unconstrained Optimization Software by Jorge J. More, Burton S.

            
            // Garbow and Kenneth E. Hillstrom in ACM Transactions on Mathematical Software,

            
            // Vol 7(1), March 1981.

            
            class PowellsFunction {

            
             public:

            
              PowellsFunction() {

            
                x_[0] =  3.0;

            
                x_[1] = -1.0;

            
                x_[2] =  0.0;

            
                x_[3] =  1.0;


                problem_.AddResidualBlock(

            
                    new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x_[0], &x_[1]);

            
                problem_.AddResidualBlock(

            
                    new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x_[2], &x_[3]);

            
                problem_.AddResidualBlock(

            
                    new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x_[1], &x_[2]);

            
                problem_.AddResidualBlock(

            
                    new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x_[0], &x_[3]);


                options_.max_num_iterations = 10;

            
              }


              Problem* mutable_problem() { return &problem_; }

            
              Solver::Options* mutable_solver_options() { return &options_; }


             private:

            
              // Templated functions used for automatically differentiated cost

            
              // functions.

            
              class F1 {

            
               public:

            
                template <typename T> bool operator()(const T* const x1,

            
                                                      const T* const x2,

            
                                                      T* residual) const {

            
                  // f1 = x1 + 10 * x2;

            
                  *residual = *x1 + T(10.0) * *x2;

            
                  return true;

            
                }

            
              };


              class F2 {

            
               public:

            
                template <typename T> bool operator()(const T* const x3,

            
                                                      const T* const x4,

            
                                                      T* residual) const {

            
                  // f2 = sqrt(5) (x3 - x4)

            
                  *residual = T(sqrt(5.0)) * (*x3 - *x4);

            
                  return true;

            
                }

            
              };


              class F3 {

            
               public:

            
                template <typename T> bool operator()(const T* const x2,

            
                                                      const T* const x4,

            
                                                      T* residual) const {

            
                  // f3 = (x2 - 2 x3)^2

            
                  residual[0] = (x2[0] - T(2.0) * x4[0]) * (x2[0] - T(2.0) * x4[0]);

            
                  return true;

            
                }

            
              };


              class F4 {

            
               public:

            
                template <typename T> bool operator()(const T* const x1,

            
                                                      const T* const x4,

            
                                                      T* residual) const {

            
                  // f4 = sqrt(10) (x1 - x4)^2

            
                  residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]);

            
                  return true;

            
                }

            
              };


              double x_[4];

            
              Problem problem_;

            
              Solver::Options options_;

            
            };


            TEST(SystemTest, PowellsFunction) {

            
              vector<SolverConfig> configs;

            
            #define CONFIGURE(linear_solver, sparse_linear_algebra_library, ordering) \

            
              configs.push_back(SolverConfig(linear_solver,                           \

            
                                             sparse_linear_algebra_library,           \

            
                                             ordering))


              CONFIGURE(DENSE_QR,    SUITE_SPARSE, NATURAL);

            
              CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, SCHUR);


            #ifndef CERES_NO_SUITESPARSE

            
              CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, NATURAL);

            
              CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, SCHUR);

            
            #endif  // CERES_NO_SUITESPARSE


            #ifndef CERES_NO_CXSPARSE

            
              CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, NATURAL);

            
              CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, SCHUR);

            
            #endif  // CERES_NO_CXSPARSE


              CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, SCHUR);


            #undef CONFIGURE


              const double kMaxAbsoluteDifference = 1e-8;

            
              RunSolversAndCheckTheyMatch<PowellsFunction>(configs, kMaxAbsoluteDifference);

            
            }


            // This class implements the SystemTestProblem interface and provides

            
            // access to a bundle adjustment problem. It is based on

            
            // examples/bundle_adjustment_example.cc. Currently a small 16 camera

            
            // problem is hard coded in the constructor. Going forward we may

            
            // extend this to a larger number of problems.

            
            class BundleAdjustmentProblem {

            
             public:

            
              BundleAdjustmentProblem() {

            
                const string input_file = JoinPath(FLAGS_test_srcdir,

            
                                                   "problem-16-22106-pre.txt");

            
                ReadData(input_file);

            
                BuildProblem();

            
              }


              ~BundleAdjustmentProblem() {

            
                delete []point_index_;

            
                delete []camera_index_;

            
                delete []observations_;

            
                delete []parameters_;

            
              }


              Problem* mutable_problem() { return &problem_; }

            
              Solver::Options* mutable_solver_options() { return &options_; }


              int num_cameras()            const { return num_cameras_;        }

            
              int num_points()             const { return num_points_;         }

            
              int num_observations()       const { return num_observations_;   }

            
              const int* point_index()     const { return point_index_;  }

            
              const int* camera_index()    const { return camera_index_; }

            
              const double* observations() const { return observations_; }

            
              double* mutable_cameras() { return parameters_; }

            
              double* mutable_points() { return parameters_  + 9 * num_cameras_; }


             private:

            
              void ReadData(const string& filename) {

            
                FILE * fptr = fopen(filename.c_str(), "r");


                if (!fptr) {

            
                  LOG(FATAL) << "File Error: unable to open file " << filename;

            
                };


                // This will die horribly on invalid files. Them's the breaks.

            
                FscanfOrDie(fptr, "%d", &num_cameras_);

            
                FscanfOrDie(fptr, "%d", &num_points_);

            
                FscanfOrDie(fptr, "%d", &num_observations_);


                VLOG(1) << "Header: " << num_cameras_

            
                        << " " << num_points_

            
                        << " " << num_observations_;


                point_index_ = new int[num_observations_];

            
                camera_index_ = new int[num_observations_];

            
                observations_ = new double[2 * num_observations_];


                num_parameters_ = 9 * num_cameras_ + 3 * num_points_;

            
                parameters_ = new double[num_parameters_];


                for (int i = 0; i < num_observations_; ++i) {

            
                  FscanfOrDie(fptr, "%d", camera_index_ + i);

            
                  FscanfOrDie(fptr, "%d", point_index_ + i);

            
                  for (int j = 0; j < 2; ++j) {

            
                    FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);

            
                  }

            
                }


                for (int i = 0; i < num_parameters_; ++i) {

            
                  FscanfOrDie(fptr, "%lf", parameters_ + i);

            
                }

            
              }


              void BuildProblem() {

            
                double* points = mutable_points();

            
                double* cameras = mutable_cameras();


                for (int i = 0; i < num_observations(); ++i) {

            
                  // Each Residual block takes a point and a camera as input and

            
                  // outputs a 2 dimensional residual.

            
                  CostFunction* cost_function =

            
                      new AutoDiffCostFunction<BundlerResidual, 2, 9, 3>(

            
                          new BundlerResidual(observations_[2*i + 0],

            
                                              observations_[2*i + 1]));


                  // Each observation correponds to a pair of a camera and a point

            
                  // which are identified by camera_index()[i] and

            
                  // point_index()[i] respectively.

            
                  double* camera = cameras + 9 * camera_index_[i];

            
                  double* point = points + 3 * point_index()[i];

            
                  problem_.AddResidualBlock(cost_function, NULL, camera, point);

            
                }


                // The points come before the cameras.

            
                for (int i = 0; i < num_points_; ++i) {

            
                  options_.ordering.push_back(points + 3 * i);

            
                }


                for (int i = 0; i < num_cameras_; ++i) {

            
                  options_.ordering.push_back(cameras + 9 * i);

            
                }


                options_.num_eliminate_blocks = num_points();

            
                options_.max_num_iterations = 25;

            
                options_.function_tolerance = 1e-10;

            
                options_.gradient_tolerance = 1e-10;

            
                options_.parameter_tolerance = 1e-10;

            
              }


              template<typename T>

            
              void FscanfOrDie(FILE *fptr, const char *format, T *value) {

            
                int num_scanned = fscanf(fptr, format, value);

            
                if (num_scanned != 1) {

            
                  LOG(FATAL) << "Invalid UW data file.";

            
                }

            
              }


              // Templated pinhole camera model.  The camera is parameterized

            
              // using 9 parameters. 3 for rotation, 3 for translation, 1 for

            
              // focal length and 2 for radial distortion. The principal point is

            
              // not modeled (i.e. it is assumed be located at the image center).

            
              struct BundlerResidual {

            
                // (u, v): the position of the observation with respect to the image

            
                // center point.

            
                BundlerResidual(double u, double v): u(u), v(v) {}


                template <typename T>

            
                bool operator()(const T* const camera,

            
                                const T* const point,

            
                                T* residuals) const {

            
                  T p[3];

            
                  AngleAxisRotatePoint(camera, point, p);


                  // Add the translation vector

            
                  p[0] += camera[3];

            
                  p[1] += camera[4];

            
                  p[2] += camera[5];


                  const T& focal = camera[6];

            
                  const T& l1 = camera[7];

            
                  const T& l2 = camera[8];


                  // Compute the center of distortion.  The sign change comes from

            
                  // the camera model that Noah Snavely's Bundler assumes, whereby

            
                  // the camera coordinate system has a negative z axis.

            
                  T xp = - focal * p[0] / p[2];

            
                  T yp = - focal * p[1] / p[2];


                  // Apply second and fourth order radial distortion.

            
                  T r2 = xp*xp + yp*yp;

            
                  T distortion = T(1.0) + r2  * (l1 + l2  * r2);


                  residuals[0] = distortion * xp - T(u);

            
                  residuals[1] = distortion * yp - T(v);


                  return true;

            
                }


                double u;

            
                double v;

            
              };


              Problem problem_;

            
              Solver::Options options_;


              int num_cameras_;

            
              int num_points_;

            
              int num_observations_;

            
              int num_parameters_;


              int* point_index_;

            
              int* camera_index_;

            
              double* observations_;

            
              // The parameter vector is laid out as follows

            
              // [camera_1, ..., camera_n, point_1, ..., point_m]

            
              double* parameters_;

            
            };


            TEST(SystemTest, BundleAdjustmentProblem) {

            
              vector<SolverConfig> configs;


            #define CONFIGURE(linear_solver, sparse_linear_algebra_library, ordering, preconditioner, threads) \

            
              configs.push_back(SolverConfig(linear_solver,                         \

            
                                             sparse_linear_algebra_library,         \

            
                                             ordering,                              \

            
                                             preconditioner,                        \

            
                                             threads))


            #ifndef CERES_NO_SUITESPARSE

            
              CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, NATURAL, IDENTITY, 1);

            
              CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, USER,    IDENTITY, 1);

            
              CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, SCHUR,   IDENTITY, 1);


              CONFIGURE(SPARSE_SCHUR,           SUITE_SPARSE, USER,    IDENTITY, 1);

            
              CONFIGURE(SPARSE_SCHUR,           SUITE_SPARSE, SCHUR,   IDENTITY, 1);

            
            #endif  // CERES_NO_SUITESPARSE


            #ifndef CERES_NO_CXSPARSE

            
              CONFIGURE(SPARSE_SCHUR,           CX_SPARSE, USER,       IDENTITY, 1);

            
              CONFIGURE(SPARSE_SCHUR,           CX_SPARSE, SCHUR,      IDENTITY, 1);

            
            #endif  // CERES_NO_CXSPARSE


              CONFIGURE(DENSE_SCHUR,            SUITE_SPARSE, USER,    IDENTITY, 1);

            
              CONFIGURE(DENSE_SCHUR,            SUITE_SPARSE, SCHUR,   IDENTITY, 1);


              CONFIGURE(CGNR,                   SUITE_SPARSE, USER,    JACOBI, 1);


              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, USER,    JACOBI, 1);


            #ifndef CERES_NO_SUITESPARSE

            
              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, USER,    SCHUR_JACOBI, 1);

            
              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, USER,    CLUSTER_JACOBI, 1);

            
              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, USER,    CLUSTER_TRIDIAGONAL, 1);

            
            #endif  // CERES_NO_SUITESPARSE


              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, SCHUR,   JACOBI, 1);


            #ifndef CERES_NO_SUITESPARSE

            
              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, SCHUR,   SCHUR_JACOBI, 1);

            
              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, SCHUR,   CLUSTER_JACOBI, 1);

            
              CONFIGURE(ITERATIVE_SCHUR,        SUITE_SPARSE, SCHUR,   CLUSTER_TRIDIAGONAL, 1);

            
            #endif  // CERES_NO_SUITESPARSE


            #undef CONFIGURE


              // Single threaded evaluators and linear solvers.

            
              const double kMaxAbsoluteDifference = 1e-4;

            
              RunSolversAndCheckTheyMatch<BundleAdjustmentProblem>(configs,

            
                                                                   kMaxAbsoluteDifference);


            #ifdef CERES_USE_OPENMP

            
              // Multithreaded evaluators and linear solvers.

            
              for (int i = 0; i < configs.size(); ++i) {

            
                configs[i].num_threads = 2;

            
              }

            
              RunSolversAndCheckTheyMatch<BundleAdjustmentProblem>(configs,

            
                                                                   kMaxAbsoluteDifference);

            
            #endif  // CERES_USE_OPENMP

            
            }


            }  // namespace internal

            
            }  // namespace ceres
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
	// http://code.google.com/p/ceres-solver/
	//
	// 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: keir@google.com (Keir Mierle)
	// sameeragarwal@google.com (Sameer Agarwal)
	//
	// System level tests for Ceres. The current suite of two tests. The
	// first test is a small test based on Powell's Function. It is a
	// scalar problem with 4 variables. The second problem is a bundle
	// adjustment problem with 16 cameras and two thousand cameras. The
	// first problem is to test the sanity test the factorization based
	// solvers. The second problem is used to test the various
	// combinations of solvers, orderings, preconditioners and
	// multithreading.

	#include <cmath>
	#include <cstdio>
	#include <cstdlib>
	#include <string>

	#include <glog/logging.h>
	#include "ceres/file.h"
	#include "gflags/gflags.h"
	#include "gtest/gtest.h"
	#include "ceres/stringprintf.h"
	#include "ceres/test_util.h"
	#include "ceres/autodiff_cost_function.h"
	#include "ceres/problem.h"
	#include "ceres/solver.h"
	#include "ceres/types.h"
	#include "ceres/rotation.h"

	DECLARE_string(test_srcdir);

	namespace ceres {
	namespace internal {

	// Struct used for configuring the solver.
	struct SolverConfig {
	SolverConfig(LinearSolverType linear_solver_type,
	SparseLinearAlgebraLibraryType sparse_linear_algebra_library,
	OrderingType ordering_type)
	: linear_solver_type(linear_solver_type),
	sparse_linear_algebra_library(sparse_linear_algebra_library),
	ordering_type(ordering_type),
	preconditioner_type(IDENTITY),
	num_threads(1) {
	}

	SolverConfig(LinearSolverType linear_solver_type,
	SparseLinearAlgebraLibraryType sparse_linear_algebra_library,
	OrderingType ordering_type,
	PreconditionerType preconditioner_type,
	int num_threads)
	: linear_solver_type(linear_solver_type),
	sparse_linear_algebra_library(sparse_linear_algebra_library),
	ordering_type(ordering_type),
	preconditioner_type(preconditioner_type),
	num_threads(num_threads) {
	}

	string ToString() const {
	return StringPrintf(
	"(%s, %s, %s, %s, %d)",
	LinearSolverTypeToString(linear_solver_type),
	SparseLinearAlgebraLibraryTypeToString(sparse_linear_algebra_library),
	OrderingTypeToString(ordering_type),
	PreconditionerTypeToString(preconditioner_type),
	num_threads);
	}

	LinearSolverType linear_solver_type;
	SparseLinearAlgebraLibraryType sparse_linear_algebra_library;
	OrderingType ordering_type;
	PreconditionerType preconditioner_type;
	int num_threads;
	};

	// Templated function that given a set of solver configurations,
	// instantiates a new copy of SystemTestProblem for each configuration
	// and solves it. The solutions are expected to have residuals with
	// coordinate-wise maximum absolute difference less than or equal to
	// max_abs_difference.
	//
	// The template parameter SystemTestProblem is expected to implement
	// the following interface.
	//
	// class SystemTestProblem {
	// public:
	// SystemTestProblem();
	// Problem* mutable_problem();
	// Solver::Options* mutable_solver_options();
	// };
	template <typename SystemTestProblem>
	void RunSolversAndCheckTheyMatch(const vector<SolverConfig>& configurations,
	const double max_abs_difference) {
	int num_configurations = configurations.size();
	vector<SystemTestProblem*> problems;
	vector<Solver::Summary> summaries(num_configurations);

	for (int i = 0; i < num_configurations; ++i) {
	SystemTestProblem* system_test_problem = new SystemTestProblem();

	const SolverConfig& config = configurations[i];

	Solver::Options& options = *(system_test_problem->mutable_solver_options());
	options.linear_solver_type = config.linear_solver_type;
	options.sparse_linear_algebra_library =
	config.sparse_linear_algebra_library;
	options.ordering_type = config.ordering_type;
	options.preconditioner_type = config.preconditioner_type;
	options.num_threads = config.num_threads;
	options.num_linear_solver_threads = config.num_threads;
	options.return_final_residuals = true;

	if (options.ordering_type == SCHUR \|\| options.ordering_type == NATURAL) {
	options.ordering.clear();
	}

	if (options.ordering_type == SCHUR) {
	options.num_eliminate_blocks = 0;
	}

	LOG(INFO) << "Running solver configuration: "
	<< config.ToString();

	Solve(options,
	system_test_problem->mutable_problem(),
	&summaries[i]);

	CHECK_NE(summaries[i].termination_type, ceres::NUMERICAL_FAILURE)
	<< "Solver configuration " << i << " failed.";
	problems.push_back(system_test_problem);

	// Compare the resulting solutions to each other. Arbitrarily take
	// SPARSE_NORMAL_CHOLESKY as the golden solve. We compare
	// solutions by comparing their residual vectors. We do not
	// compare parameter vectors because it is much more brittle and
	// error prone to do so, since the same problem can have nearly
	// the same residuals at two completely different positions in
	// parameter space.
	if (i > 0) {
	const vector<double>& reference_residuals = summaries[0].final_residuals;
	const vector<double>& current_residuals = summaries[i].final_residuals;

	for (int j = 0; j < reference_residuals.size(); ++j) {
	EXPECT_NEAR(current_residuals[j],
	reference_residuals[j],
	max_abs_difference)
	<< "Not close enough residual:" << j
	<< " reference " << reference_residuals[j]
	<< " current " << current_residuals[j];
	}
	}
	}

	for (int i = 0; i < num_configurations; ++i) {
	delete problems[i];
	}
	}

	// This class implements the SystemTestProblem interface and provides
	// access to an implementation of Powell's singular function.
	//
	// F = 1/2 (f1^2 + f2^2 + f3^2 + f4^2)
	//
	// f1 = x1 + 10*x2;
	// f2 = sqrt(5) * (x3 - x4)
	// f3 = (x2 - 2*x3)^2
	// f4 = sqrt(10) * (x1 - x4)^2
	//
	// The starting values are x1 = 3, x2 = -1, x3 = 0, x4 = 1.
	// The minimum is 0 at (x1, x2, x3, x4) = 0.
	//
	// From: Testing Unconstrained Optimization Software by Jorge J. More, Burton S.
	// Garbow and Kenneth E. Hillstrom in ACM Transactions on Mathematical Software,
	// Vol 7(1), March 1981.
	class PowellsFunction {
	public:
	PowellsFunction() {
	x_[0] = 3.0;
	x_[1] = -1.0;
	x_[2] = 0.0;
	x_[3] = 1.0;

	problem_.AddResidualBlock(
	new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x_[0], &x_[1]);
	problem_.AddResidualBlock(
	new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x_[2], &x_[3]);
	problem_.AddResidualBlock(
	new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x_[1], &x_[2]);
	problem_.AddResidualBlock(
	new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x_[0], &x_[3]);

	options_.max_num_iterations = 10;
	}

	Problem* mutable_problem() { return &problem_; }
	Solver::Options* mutable_solver_options() { return &options_; }

	private:
	// Templated functions used for automatically differentiated cost
	// functions.
	class F1 {
	public:
	template <typename T> bool operator()(const T* const x1,
	const T* const x2,
	T* residual) const {
	// f1 = x1 + 10 * x2;
	residual = x1 + T(10.0) * *x2;
	return true;
	}
	};

	class F2 {
	public:
	template <typename T> bool operator()(const T* const x3,
	const T* const x4,
	T* residual) const {
	// f2 = sqrt(5) (x3 - x4)
	residual = T(sqrt(5.0)) (x3 - x4);
	return true;
	}
	};

	class F3 {
	public:
	template <typename T> bool operator()(const T* const x2,
	const T* const x4,
	T* residual) const {
	// f3 = (x2 - 2 x3)^2
	residual[0] = (x2[0] - T(2.0) * x4[0]) * (x2[0] - T(2.0) * x4[0]);
	return true;
	}
	};

	class F4 {
	public:
	template <typename T> bool operator()(const T* const x1,
	const T* const x4,
	T* residual) const {
	// f4 = sqrt(10) (x1 - x4)^2
	residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
	return true;
	}
	};

	double x_[4];
	Problem problem_;
	Solver::Options options_;
	};

	TEST(SystemTest, PowellsFunction) {
	vector<SolverConfig> configs;
	#define CONFIGURE(linear_solver, sparse_linear_algebra_library, ordering) \
	configs.push_back(SolverConfig(linear_solver, \
	sparse_linear_algebra_library, \
	ordering))

	CONFIGURE(DENSE_QR, SUITE_SPARSE, NATURAL);
	CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, SCHUR);

	#ifndef CERES_NO_SUITESPARSE
	CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, NATURAL);
	CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, SCHUR);
	#endif // CERES_NO_SUITESPARSE

	#ifndef CERES_NO_CXSPARSE
	CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, NATURAL);
	CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, SCHUR);
	#endif // CERES_NO_CXSPARSE

	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, SCHUR);

	#undef CONFIGURE

	const double kMaxAbsoluteDifference = 1e-8;
	RunSolversAndCheckTheyMatch<PowellsFunction>(configs, kMaxAbsoluteDifference);
	}

	// This class implements the SystemTestProblem interface and provides
	// access to a bundle adjustment problem. It is based on
	// examples/bundle_adjustment_example.cc. Currently a small 16 camera
	// problem is hard coded in the constructor. Going forward we may
	// extend this to a larger number of problems.
	class BundleAdjustmentProblem {
	public:
	BundleAdjustmentProblem() {
	const string input_file = JoinPath(FLAGS_test_srcdir,
	"problem-16-22106-pre.txt");
	ReadData(input_file);
	BuildProblem();
	}

	~BundleAdjustmentProblem() {
	delete []point_index_;
	delete []camera_index_;
	delete []observations_;
	delete []parameters_;
	}

	Problem* mutable_problem() { return &problem_; }
	Solver::Options* mutable_solver_options() { return &options_; }

	int num_cameras() const { return num_cameras_; }
	int num_points() const { return num_points_; }
	int num_observations() const { return num_observations_; }
	const int* point_index() const { return point_index_; }
	const int* camera_index() const { return camera_index_; }
	const double* observations() const { return observations_; }
	double* mutable_cameras() { return parameters_; }
	double* mutable_points() { return parameters_ + 9 * num_cameras_; }

	private:
	void ReadData(const string& filename) {
	FILE * fptr = fopen(filename.c_str(), "r");

	if (!fptr) {
	LOG(FATAL) << "File Error: unable to open file " << filename;
	};

	// This will die horribly on invalid files. Them's the breaks.
	FscanfOrDie(fptr, "%d", &num_cameras_);
	FscanfOrDie(fptr, "%d", &num_points_);
	FscanfOrDie(fptr, "%d", &num_observations_);

	VLOG(1) << "Header: " << num_cameras_
	<< " " << num_points_
	<< " " << num_observations_;

	point_index_ = new int[num_observations_];
	camera_index_ = new int[num_observations_];
	observations_ = new double[2 * num_observations_];

	num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
	parameters_ = new double[num_parameters_];

	for (int i = 0; i < num_observations_; ++i) {
	FscanfOrDie(fptr, "%d", camera_index_ + i);
	FscanfOrDie(fptr, "%d", point_index_ + i);
	for (int j = 0; j < 2; ++j) {
	FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
	}
	}

	for (int i = 0; i < num_parameters_; ++i) {
	FscanfOrDie(fptr, "%lf", parameters_ + i);
	}
	}

	void BuildProblem() {
	double* points = mutable_points();
	double* cameras = mutable_cameras();

	for (int i = 0; i < num_observations(); ++i) {
	// Each Residual block takes a point and a camera as input and
	// outputs a 2 dimensional residual.
	CostFunction* cost_function =
	new AutoDiffCostFunction<BundlerResidual, 2, 9, 3>(
	new BundlerResidual(observations_[2*i + 0],
	observations_[2*i + 1]));

	// Each observation correponds to a pair of a camera and a point
	// which are identified by camera_index()[i] and
	// point_index()[i] respectively.
	double* camera = cameras + 9 * camera_index_[i];
	double* point = points + 3 * point_index()[i];
	problem_.AddResidualBlock(cost_function, NULL, camera, point);
	}

	// The points come before the cameras.
	for (int i = 0; i < num_points_; ++i) {
	options_.ordering.push_back(points + 3 * i);
	}

	for (int i = 0; i < num_cameras_; ++i) {
	options_.ordering.push_back(cameras + 9 * i);
	}

	options_.num_eliminate_blocks = num_points();
	options_.max_num_iterations = 25;
	options_.function_tolerance = 1e-10;
	options_.gradient_tolerance = 1e-10;
	options_.parameter_tolerance = 1e-10;
	}

	template<typename T>
	void FscanfOrDie(FILE fptr, const char format, T *value) {
	int num_scanned = fscanf(fptr, format, value);
	if (num_scanned != 1) {
	LOG(FATAL) << "Invalid UW data file.";
	}
	}

	// Templated pinhole camera model. The camera is parameterized
	// using 9 parameters. 3 for rotation, 3 for translation, 1 for
	// focal length and 2 for radial distortion. The principal point is
	// not modeled (i.e. it is assumed be located at the image center).
	struct BundlerResidual {
	// (u, v): the position of the observation with respect to the image
	// center point.
	BundlerResidual(double u, double v): u(u), v(v) {}

	template <typename T>
	bool operator()(const T* const camera,
	const T* const point,
	T* residuals) const {
	T p[3];
	AngleAxisRotatePoint(camera, point, p);

	// Add the translation vector
	p[0] += camera[3];
	p[1] += camera[4];
	p[2] += camera[5];

	const T& focal = camera[6];
	const T& l1 = camera[7];
	const T& l2 = camera[8];

	// Compute the center of distortion. The sign change comes from
	// the camera model that Noah Snavely's Bundler assumes, whereby
	// the camera coordinate system has a negative z axis.
	T xp = - focal * p[0] / p[2];
	T yp = - focal * p[1] / p[2];

	// Apply second and fourth order radial distortion.
	T r2 = xpxp + ypyp;
	T distortion = T(1.0) + r2 * (l1 + l2 * r2);

	residuals[0] = distortion * xp - T(u);
	residuals[1] = distortion * yp - T(v);

	return true;
	}

	double u;
	double v;
	};


	Problem problem_;
	Solver::Options options_;

	int num_cameras_;
	int num_points_;
	int num_observations_;
	int num_parameters_;

	int* point_index_;
	int* camera_index_;
	double* observations_;
	// The parameter vector is laid out as follows
	// [camera_1, ..., camera_n, point_1, ..., point_m]
	double* parameters_;
	};

	TEST(SystemTest, BundleAdjustmentProblem) {
	vector<SolverConfig> configs;

	#define CONFIGURE(linear_solver, sparse_linear_algebra_library, ordering, preconditioner, threads) \
	configs.push_back(SolverConfig(linear_solver, \
	sparse_linear_algebra_library, \
	ordering, \
	preconditioner, \
	threads))

	#ifndef CERES_NO_SUITESPARSE
	CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, NATURAL, IDENTITY, 1);
	CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, USER, IDENTITY, 1);
	CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, SCHUR, IDENTITY, 1);

	CONFIGURE(SPARSE_SCHUR, SUITE_SPARSE, USER, IDENTITY, 1);
	CONFIGURE(SPARSE_SCHUR, SUITE_SPARSE, SCHUR, IDENTITY, 1);
	#endif // CERES_NO_SUITESPARSE

	#ifndef CERES_NO_CXSPARSE
	CONFIGURE(SPARSE_SCHUR, CX_SPARSE, USER, IDENTITY, 1);
	CONFIGURE(SPARSE_SCHUR, CX_SPARSE, SCHUR, IDENTITY, 1);
	#endif // CERES_NO_CXSPARSE

	CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, USER, IDENTITY, 1);
	CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, SCHUR, IDENTITY, 1);

	CONFIGURE(CGNR, SUITE_SPARSE, USER, JACOBI, 1);

	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, USER, JACOBI, 1);

	#ifndef CERES_NO_SUITESPARSE
	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, USER, SCHUR_JACOBI, 1);
	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, USER, CLUSTER_JACOBI, 1);
	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, USER, CLUSTER_TRIDIAGONAL, 1);
	#endif // CERES_NO_SUITESPARSE

	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, SCHUR, JACOBI, 1);

	#ifndef CERES_NO_SUITESPARSE
	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, SCHUR, SCHUR_JACOBI, 1);
	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, SCHUR, CLUSTER_JACOBI, 1);
	CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, SCHUR, CLUSTER_TRIDIAGONAL, 1);
	#endif // CERES_NO_SUITESPARSE

	#undef CONFIGURE

	// Single threaded evaluators and linear solvers.
	const double kMaxAbsoluteDifference = 1e-4;
	RunSolversAndCheckTheyMatch<BundleAdjustmentProblem>(configs,
	kMaxAbsoluteDifference);

	#ifdef CERES_USE_OPENMP
	// Multithreaded evaluators and linear solvers.
	for (int i = 0; i < configs.size(); ++i) {
	configs[i].num_threads = 2;
	}
	RunSolversAndCheckTheyMatch<BundleAdjustmentProblem>(configs,
	kMaxAbsoluteDifference);
	#endif // CERES_USE_OPENMP
	}

	} // namespace internal
	} // namespace ceres