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

 // Ceres Solver - A fast non-linear least squares minimizer
 // Copyright 2015 Google Inc. All rights reserved.
 // http://ceres-solver.org/
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
 //
 // * Redistributions of source code must retain the above copyright notice,
 //   this list of conditions and the following disclaimer.
 // * Redistributions in binary form must reproduce the above copyright notice,
 //   this list of conditions and the following disclaimer in the documentation
 //   and/or other materials provided with the distribution.
 // * Neither the name of Google Inc. nor the names of its contributors may be
 //   used to endorse or promote products derived from this software without
 //   specific prior written permission.
 //
 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
 // Author: keir@google.com (Keir Mierle)

 #include "ceres/gradient_checking_cost_function.h"

 #include <cmath>
 #include <cstdint>
 #include <memory>
 #include <vector>

 #include "ceres/cost_function.h"
 #include "ceres/local_parameterization.h"
 #include "ceres/loss_function.h"
 #include "ceres/parameter_block.h"
 #include "ceres/problem_impl.h"
 #include "ceres/program.h"
 #include "ceres/random.h"
 #include "ceres/residual_block.h"
 #include "ceres/sized_cost_function.h"
 #include "ceres/types.h"
 #include "glog/logging.h"
 #include "gmock/gmock.h"
 #include "gtest/gtest.h"

 namespace ceres {
 namespace internal {

 using std::vector;
 using testing::AllOf;
 using testing::AnyNumber;
 using testing::HasSubstr;
 using testing::_;

 // Pick a (non-quadratic) function whose derivative are easy:
 //
 //    f = exp(- a' x).
 //   df = - f a.
 //
 // where 'a' is a vector of the same size as 'x'. In the block
 // version, they are both block vectors, of course.
 template<int bad_block = 1, int bad_variable = 2>
 class TestTerm : public CostFunction {
  public:
   // The constructor of this function needs to know the number
   // of blocks desired, and the size of each block.
   TestTerm(int arity, int const *dim) : arity_(arity) {
     // Make 'arity' random vectors.
     a_.resize(arity_);
     for (int j = 0; j < arity_; ++j) {
       a_[j].resize(dim[j]);
       for (int u = 0; u < dim[j]; ++u) {
         a_[j][u] = 2.0 * RandDouble() - 1.0;
       }
     }

     for (int i = 0; i < arity_; i++) {
       mutable_parameter_block_sizes()->push_back(dim[i]);
     }
     set_num_residuals(1);
   }

   bool Evaluate(double const* const* parameters,
                 double* residuals,
                 double** jacobians) const {
     // Compute a . x.
     double ax = 0;
     for (int j = 0; j < arity_; ++j) {
       for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
         ax += a_[j][u] * parameters[j][u];
       }
     }

     // This is the cost, but also appears as a factor
     // in the derivatives.
     double f = *residuals = exp(-ax);

     // Accumulate 1st order derivatives.
     if (jacobians) {
       for (int j = 0; j < arity_; ++j) {
         if (jacobians[j]) {
           for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
             // See comments before class.
             jacobians[j][u] = - f * a_[j][u];

             if (bad_block == j && bad_variable == u) {
               // Whoopsiedoopsie! Deliberately introduce a faulty jacobian entry
               // like what happens when users make an error in their jacobian
               // computations. This should get detected.
               LOG(INFO) << "Poisoning jacobian for parameter block " << j
                         << ", row 0, column " << u;
               jacobians[j][u] += 500;
             }
           }
         }
       }
     }

     return true;
   }

  private:
   int arity_;
   vector<vector<double>> a_;
 };

 TEST(GradientCheckingCostFunction, ResidualsAndJacobiansArePreservedTest) {
   srand(5);

   // Test with 3 blocks of size 2, 3 and 4.
   int const arity = 3;
   int const dim[arity] = { 2, 3, 4 };

   // Make a random set of blocks.
   vector<double*> parameters(arity);
   for (int j = 0; j < arity; ++j) {
     parameters[j] = new double[dim[j]];
     for (int u = 0; u < dim[j]; ++u) {
       parameters[j][u] = 2.0 * RandDouble() - 1.0;
     }
   }

   double original_residual;
   double residual;
   vector<double*> original_jacobians(arity);
   vector<double*> jacobians(arity);

   for (int j = 0; j < arity; ++j) {
     // Since residual is one dimensional the jacobians have the same
     // size as the parameter blocks.
     jacobians[j] = new double[dim[j]];
     original_jacobians[j] = new double[dim[j]];
   }

   const double kRelativeStepSize = 1e-6;
   const double kRelativePrecision = 1e-4;

   TestTerm<-1, -1> term(arity, dim);
   GradientCheckingIterationCallback callback;
   std::unique_ptr<CostFunction> gradient_checking_cost_function(
       CreateGradientCheckingCostFunction(&term, NULL,
                                          kRelativeStepSize,
                                          kRelativePrecision,
                                          "Ignored.", &callback));
   term.Evaluate(&parameters[0],
                 &original_residual,
                 &original_jacobians[0]);

   gradient_checking_cost_function->Evaluate(&parameters[0],
                                             &residual,
                                             &jacobians[0]);
   EXPECT_EQ(original_residual, residual);

   for (int j = 0; j < arity; j++) {
     for (int k = 0; k < dim[j]; ++k) {
       EXPECT_EQ(original_jacobians[j][k], jacobians[j][k]);
     }

     delete[] parameters[j];
     delete[] jacobians[j];
     delete[] original_jacobians[j];
   }
 }

 TEST(GradientCheckingCostFunction, SmokeTest) {
   srand(5);

   // Test with 3 blocks of size 2, 3 and 4.
   int const arity = 3;
   int const dim[arity] = { 2, 3, 4 };

   // Make a random set of blocks.
   vector<double*> parameters(arity);
   for (int j = 0; j < arity; ++j) {
     parameters[j] = new double[dim[j]];
     for (int u = 0; u < dim[j]; ++u) {
       parameters[j][u] = 2.0 * RandDouble() - 1.0;
     }
   }

   double residual;
   vector<double*> jacobians(arity);
   for (int j = 0; j < arity; ++j) {
     // Since residual is one dimensional the jacobians have the same size as the
     // parameter blocks.
     jacobians[j] = new double[dim[j]];
   }

   const double kRelativeStepSize = 1e-6;
   const double kRelativePrecision = 1e-4;

   // Should have one term that's bad, causing everything to get dumped.
   LOG(INFO) << "Bad gradient";
   {
     TestTerm<1, 2> term(arity, dim);
     GradientCheckingIterationCallback callback;
     std::unique_ptr<CostFunction> gradient_checking_cost_function(
         CreateGradientCheckingCostFunction(&term, NULL,
                                            kRelativeStepSize,
                                            kRelativePrecision,
                                            "Fuzzy banana", &callback));
     EXPECT_TRUE(
         gradient_checking_cost_function->Evaluate(&parameters[0], &residual,
                                                   &jacobians[0]));
     EXPECT_TRUE(callback.gradient_error_detected());
     EXPECT_TRUE(callback.error_log().find("Fuzzy banana") != std::string::npos);
     EXPECT_TRUE(callback.error_log().find("(1,0,2) Relative error worse than")
                 != std::string::npos);
   }

   // The gradient is correct, so no errors are reported.
   LOG(INFO) << "Good gradient";
   {
     TestTerm<-1, -1> term(arity, dim);
     GradientCheckingIterationCallback callback;
     std::unique_ptr<CostFunction> gradient_checking_cost_function(
         CreateGradientCheckingCostFunction(&term, NULL,
                                            kRelativeStepSize,
                                            kRelativePrecision,
                                            "Fuzzy banana", &callback));
     EXPECT_TRUE(
         gradient_checking_cost_function->Evaluate(&parameters[0], &residual,
                                                   &jacobians[0]));
     EXPECT_FALSE(callback.gradient_error_detected());
   }

   for (int j = 0; j < arity; j++) {
     delete[] parameters[j];
     delete[] jacobians[j];
   }
 }

 // The following three classes are for the purposes of defining
 // function signatures. They have dummy Evaluate functions.

 // Trivial cost function that accepts a single argument.
 class UnaryCostFunction : public CostFunction {
  public:
   UnaryCostFunction(int num_residuals, int32_t parameter_block_size) {
     set_num_residuals(num_residuals);
     mutable_parameter_block_sizes()->push_back(parameter_block_size);
   }
   virtual ~UnaryCostFunction() {}

   bool Evaluate(double const* const* parameters,
                 double* residuals,
                 double** jacobians) const final {
     for (int i = 0; i < num_residuals(); ++i) {
       residuals[i] = 1;
     }
     return true;
   }
 };

 // Trivial cost function that accepts two arguments.
 class BinaryCostFunction: public CostFunction {
  public:
   BinaryCostFunction(int num_residuals,
                      int32_t parameter_block1_size,
                      int32_t parameter_block2_size) {
     set_num_residuals(num_residuals);
     mutable_parameter_block_sizes()->push_back(parameter_block1_size);
     mutable_parameter_block_sizes()->push_back(parameter_block2_size);
   }

   bool Evaluate(double const* const* parameters,
                 double* residuals,
                 double** jacobians) const final {
     for (int i = 0; i < num_residuals(); ++i) {
       residuals[i] = 2;
     }
     return true;
   }
 };

 // Trivial cost function that accepts three arguments.
 class TernaryCostFunction: public CostFunction {
  public:
   TernaryCostFunction(int num_residuals,
                       int32_t parameter_block1_size,
                       int32_t parameter_block2_size,
                       int32_t parameter_block3_size) {
     set_num_residuals(num_residuals);
     mutable_parameter_block_sizes()->push_back(parameter_block1_size);
     mutable_parameter_block_sizes()->push_back(parameter_block2_size);
     mutable_parameter_block_sizes()->push_back(parameter_block3_size);
   }

   bool Evaluate(double const* const* parameters,
                 double* residuals,
                 double** jacobians) const final {
     for (int i = 0; i < num_residuals(); ++i) {
       residuals[i] = 3;
     }
     return true;
   }
 };

 // Verify that the two ParameterBlocks are formed from the same user
 // array and have the same LocalParameterization object.
 static void ParameterBlocksAreEquivalent(const ParameterBlock*  left,
                                          const ParameterBlock* right) {
   CHECK(left != nullptr);
   CHECK(right != nullptr);
   EXPECT_EQ(left->user_state(), right->user_state());
   EXPECT_EQ(left->Size(), right->Size());
   EXPECT_EQ(left->Size(), right->Size());
   EXPECT_EQ(left->LocalSize(), right->LocalSize());
   EXPECT_EQ(left->local_parameterization(), right->local_parameterization());
   EXPECT_EQ(left->IsConstant(), right->IsConstant());
 }

 TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) {
   // Parameter blocks with arbitrarily chosen initial values.
   double x[] = {1.0, 2.0, 3.0};
   double y[] = {4.0, 5.0, 6.0, 7.0};
   double z[] = {8.0, 9.0, 10.0, 11.0, 12.0};
   double w[] = {13.0, 14.0, 15.0, 16.0};

   ProblemImpl problem_impl;
   problem_impl.AddParameterBlock(x, 3);
   problem_impl.AddParameterBlock(y, 4);
   problem_impl.SetParameterBlockConstant(y);
   problem_impl.AddParameterBlock(z, 5);
   problem_impl.AddParameterBlock(w, 4, new QuaternionParameterization);
   problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
   problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4) ,
                                 NULL, z, y);
   problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5),
                                 new TrivialLoss, x, z);
   problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3),
                                 NULL, z, x);
   problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4),
                                 NULL, z, x, y);

   GradientCheckingIterationCallback callback;
   std::unique_ptr<ProblemImpl> gradient_checking_problem_impl(
       CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback));

   // The dimensions of the two problems match.
   EXPECT_EQ(problem_impl.NumParameterBlocks(),
             gradient_checking_problem_impl->NumParameterBlocks());
   EXPECT_EQ(problem_impl.NumResidualBlocks(),
             gradient_checking_problem_impl->NumResidualBlocks());

   EXPECT_EQ(problem_impl.NumParameters(),
             gradient_checking_problem_impl->NumParameters());
   EXPECT_EQ(problem_impl.NumResiduals(),
             gradient_checking_problem_impl->NumResiduals());

   const Program& program = problem_impl.program();
   const Program& gradient_checking_program =
       gradient_checking_problem_impl->program();

   // Since we added the ParameterBlocks and ResidualBlocks explicitly,
   // they should be in the same order in the two programs. It is
   // possible that may change due to implementation changes to
   // Program. This is not expected to be the case and writing code to
   // anticipate that possibility not worth the extra complexity in
   // this test.
   for (int i = 0; i < program.parameter_blocks().size(); ++i) {
     ParameterBlocksAreEquivalent(
         program.parameter_blocks()[i],
         gradient_checking_program.parameter_blocks()[i]);
   }

   for (int i = 0; i < program.residual_blocks().size(); ++i) {
     // Compare the sizes of the two ResidualBlocks.
     const ResidualBlock* original_residual_block =
         program.residual_blocks()[i];
     const ResidualBlock* new_residual_block =
         gradient_checking_program.residual_blocks()[i];
     EXPECT_EQ(original_residual_block->NumParameterBlocks(),
               new_residual_block->NumParameterBlocks());
     EXPECT_EQ(original_residual_block->NumResiduals(),
               new_residual_block->NumResiduals());
     EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(),
               new_residual_block->NumScratchDoublesForEvaluate());

     // Verify that the ParameterBlocks for the two residuals are equivalent.
     for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) {
       ParameterBlocksAreEquivalent(
           original_residual_block->parameter_blocks()[j],
           new_residual_block->parameter_blocks()[j]);
     }
   }
 }


 TEST(GradientCheckingProblemImpl, ConstrainedProblemBoundsArePropagated) {
   // Parameter blocks with arbitrarily chosen initial values.
   double x[] = {1.0, 2.0, 3.0};
   ProblemImpl problem_impl;
   problem_impl.AddParameterBlock(x, 3);
   problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
   problem_impl.SetParameterLowerBound(x,0,0.9);
   problem_impl.SetParameterUpperBound(x,1,2.5);

   GradientCheckingIterationCallback callback;
   std::unique_ptr<ProblemImpl> gradient_checking_problem_impl(
       CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback));

   // The dimensions of the two problems match.
   EXPECT_EQ(problem_impl.NumParameterBlocks(),
             gradient_checking_problem_impl->NumParameterBlocks());
   EXPECT_EQ(problem_impl.NumResidualBlocks(),
             gradient_checking_problem_impl->NumResidualBlocks());

   EXPECT_EQ(problem_impl.NumParameters(),
             gradient_checking_problem_impl->NumParameters());
   EXPECT_EQ(problem_impl.NumResiduals(),
             gradient_checking_problem_impl->NumResiduals());

   for (int i = 0; i < 3; ++i) {
     EXPECT_EQ(problem_impl.GetParameterLowerBound(x, i),
               gradient_checking_problem_impl->GetParameterLowerBound(x, i));
     EXPECT_EQ(problem_impl.GetParameterUpperBound(x, i),
               gradient_checking_problem_impl->GetParameterUpperBound(x, i));
   }
 }

 }  // namespace internal
 }  // namespace ceres
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2015 Google Inc. All rights reserved.
	// http://ceres-solver.org/
	//
	// Redistribution and use in source and binary forms, with or without
	// modification, are permitted provided that the following conditions are met:
	//
	// * Redistributions of source code must retain the above copyright notice,
	// this list of conditions and the following disclaimer.
	// * Redistributions in binary form must reproduce the above copyright notice,
	// this list of conditions and the following disclaimer in the documentation
	// and/or other materials provided with the distribution.
	// * Neither the name of Google Inc. nor the names of its contributors may be
	// used to endorse or promote products derived from this software without
	// specific prior written permission.
	//
	// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
	// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
	// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
	// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
	// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
	// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
	// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
	// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
	// POSSIBILITY OF SUCH DAMAGE.
	//
	// Author: keir@google.com (Keir Mierle)

	#include "ceres/gradient_checking_cost_function.h"

	#include <cmath>
	#include <cstdint>
	#include <memory>
	#include <vector>

	#include "ceres/cost_function.h"
	#include "ceres/local_parameterization.h"
	#include "ceres/loss_function.h"
	#include "ceres/parameter_block.h"
	#include "ceres/problem_impl.h"
	#include "ceres/program.h"
	#include "ceres/random.h"
	#include "ceres/residual_block.h"
	#include "ceres/sized_cost_function.h"
	#include "ceres/types.h"
	#include "glog/logging.h"
	#include "gmock/gmock.h"
	#include "gtest/gtest.h"

	namespace ceres {
	namespace internal {

	using std::vector;
	using testing::AllOf;
	using testing::AnyNumber;
	using testing::HasSubstr;
	using testing::_;

	// Pick a (non-quadratic) function whose derivative are easy:
	//
	// f = exp(- a' x).
	// df = - f a.
	//
	// where 'a' is a vector of the same size as 'x'. In the block
	// version, they are both block vectors, of course.
	template<int bad_block = 1, int bad_variable = 2>
	class TestTerm : public CostFunction {
	public:
	// The constructor of this function needs to know the number
	// of blocks desired, and the size of each block.
	TestTerm(int arity, int const *dim) : arity_(arity) {
	// Make 'arity' random vectors.
	a_.resize(arity_);
	for (int j = 0; j < arity_; ++j) {
	a_[j].resize(dim[j]);
	for (int u = 0; u < dim[j]; ++u) {
	a_[j][u] = 2.0 * RandDouble() - 1.0;
	}
	}

	for (int i = 0; i < arity_; i++) {
	mutable_parameter_block_sizes()->push_back(dim[i]);
	}
	set_num_residuals(1);
	}

	bool Evaluate(double const* const* parameters,
	double* residuals,
	double** jacobians) const {
	// Compute a . x.
	double ax = 0;
	for (int j = 0; j < arity_; ++j) {
	for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
	ax += a_[j][u] * parameters[j][u];
	}
	}

	// This is the cost, but also appears as a factor
	// in the derivatives.
	double f = *residuals = exp(-ax);

	// Accumulate 1st order derivatives.
	if (jacobians) {
	for (int j = 0; j < arity_; ++j) {
	if (jacobians[j]) {
	for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
	// See comments before class.
	jacobians[j][u] = - f * a_[j][u];

	if (bad_block == j && bad_variable == u) {
	// Whoopsiedoopsie! Deliberately introduce a faulty jacobian entry
	// like what happens when users make an error in their jacobian
	// computations. This should get detected.
	LOG(INFO) << "Poisoning jacobian for parameter block " << j
	<< ", row 0, column " << u;
	jacobians[j][u] += 500;
	}
	}
	}
	}
	}

	return true;
	}

	private:
	int arity_;
	vector<vector<double>> a_;
	};

	TEST(GradientCheckingCostFunction, ResidualsAndJacobiansArePreservedTest) {
	srand(5);

	// Test with 3 blocks of size 2, 3 and 4.
	int const arity = 3;
	int const dim[arity] = { 2, 3, 4 };

	// Make a random set of blocks.
	vector<double*> parameters(arity);
	for (int j = 0; j < arity; ++j) {
	parameters[j] = new double[dim[j]];
	for (int u = 0; u < dim[j]; ++u) {
	parameters[j][u] = 2.0 * RandDouble() - 1.0;
	}
	}

	double original_residual;
	double residual;
	vector<double*> original_jacobians(arity);
	vector<double*> jacobians(arity);

	for (int j = 0; j < arity; ++j) {
	// Since residual is one dimensional the jacobians have the same
	// size as the parameter blocks.
	jacobians[j] = new double[dim[j]];
	original_jacobians[j] = new double[dim[j]];
	}

	const double kRelativeStepSize = 1e-6;
	const double kRelativePrecision = 1e-4;

	TestTerm<-1, -1> term(arity, dim);
	GradientCheckingIterationCallback callback;
	std::unique_ptr<CostFunction> gradient_checking_cost_function(
	CreateGradientCheckingCostFunction(&term, NULL,
	kRelativeStepSize,
	kRelativePrecision,
	"Ignored.", &callback));
	term.Evaluate(&parameters[0],
	&original_residual,
	&original_jacobians[0]);

	gradient_checking_cost_function->Evaluate(&parameters[0],
	&residual,
	&jacobians[0]);
	EXPECT_EQ(original_residual, residual);

	for (int j = 0; j < arity; j++) {
	for (int k = 0; k < dim[j]; ++k) {
	EXPECT_EQ(original_jacobians[j][k], jacobians[j][k]);
	}

	delete[] parameters[j];
	delete[] jacobians[j];
	delete[] original_jacobians[j];
	}
	}

	TEST(GradientCheckingCostFunction, SmokeTest) {
	srand(5);

	// Test with 3 blocks of size 2, 3 and 4.
	int const arity = 3;
	int const dim[arity] = { 2, 3, 4 };

	// Make a random set of blocks.
	vector<double*> parameters(arity);
	for (int j = 0; j < arity; ++j) {
	parameters[j] = new double[dim[j]];
	for (int u = 0; u < dim[j]; ++u) {
	parameters[j][u] = 2.0 * RandDouble() - 1.0;
	}
	}

	double residual;
	vector<double*> jacobians(arity);
	for (int j = 0; j < arity; ++j) {
	// Since residual is one dimensional the jacobians have the same size as the
	// parameter blocks.
	jacobians[j] = new double[dim[j]];
	}

	const double kRelativeStepSize = 1e-6;
	const double kRelativePrecision = 1e-4;

	// Should have one term that's bad, causing everything to get dumped.
	LOG(INFO) << "Bad gradient";
	{
	TestTerm<1, 2> term(arity, dim);
	GradientCheckingIterationCallback callback;
	std::unique_ptr<CostFunction> gradient_checking_cost_function(
	CreateGradientCheckingCostFunction(&term, NULL,
	kRelativeStepSize,
	kRelativePrecision,
	"Fuzzy banana", &callback));
	EXPECT_TRUE(
	gradient_checking_cost_function->Evaluate(&parameters[0], &residual,
	&jacobians[0]));
	EXPECT_TRUE(callback.gradient_error_detected());
	EXPECT_TRUE(callback.error_log().find("Fuzzy banana") != std::string::npos);
	EXPECT_TRUE(callback.error_log().find("(1,0,2) Relative error worse than")
	!= std::string::npos);
	}

	// The gradient is correct, so no errors are reported.
	LOG(INFO) << "Good gradient";
	{
	TestTerm<-1, -1> term(arity, dim);
	GradientCheckingIterationCallback callback;
	std::unique_ptr<CostFunction> gradient_checking_cost_function(
	CreateGradientCheckingCostFunction(&term, NULL,
	kRelativeStepSize,
	kRelativePrecision,
	"Fuzzy banana", &callback));
	EXPECT_TRUE(
	gradient_checking_cost_function->Evaluate(&parameters[0], &residual,
	&jacobians[0]));
	EXPECT_FALSE(callback.gradient_error_detected());
	}

	for (int j = 0; j < arity; j++) {
	delete[] parameters[j];
	delete[] jacobians[j];
	}
	}

	// The following three classes are for the purposes of defining
	// function signatures. They have dummy Evaluate functions.

	// Trivial cost function that accepts a single argument.
	class UnaryCostFunction : public CostFunction {
	public:
	UnaryCostFunction(int num_residuals, int32_t parameter_block_size) {
	set_num_residuals(num_residuals);
	mutable_parameter_block_sizes()->push_back(parameter_block_size);
	}
	virtual ~UnaryCostFunction() {}

	bool Evaluate(double const* const* parameters,
	double* residuals,
	double** jacobians) const final {
	for (int i = 0; i < num_residuals(); ++i) {
	residuals[i] = 1;
	}
	return true;
	}
	};

	// Trivial cost function that accepts two arguments.
	class BinaryCostFunction: public CostFunction {
	public:
	BinaryCostFunction(int num_residuals,
	int32_t parameter_block1_size,
	int32_t parameter_block2_size) {
	set_num_residuals(num_residuals);
	mutable_parameter_block_sizes()->push_back(parameter_block1_size);
	mutable_parameter_block_sizes()->push_back(parameter_block2_size);
	}

	bool Evaluate(double const* const* parameters,
	double* residuals,
	double** jacobians) const final {
	for (int i = 0; i < num_residuals(); ++i) {
	residuals[i] = 2;
	}
	return true;
	}
	};

	// Trivial cost function that accepts three arguments.
	class TernaryCostFunction: public CostFunction {
	public:
	TernaryCostFunction(int num_residuals,
	int32_t parameter_block1_size,
	int32_t parameter_block2_size,
	int32_t parameter_block3_size) {
	set_num_residuals(num_residuals);
	mutable_parameter_block_sizes()->push_back(parameter_block1_size);
	mutable_parameter_block_sizes()->push_back(parameter_block2_size);
	mutable_parameter_block_sizes()->push_back(parameter_block3_size);
	}

	bool Evaluate(double const* const* parameters,
	double* residuals,
	double** jacobians) const final {
	for (int i = 0; i < num_residuals(); ++i) {
	residuals[i] = 3;
	}
	return true;
	}
	};

	// Verify that the two ParameterBlocks are formed from the same user
	// array and have the same LocalParameterization object.
	static void ParameterBlocksAreEquivalent(const ParameterBlock* left,
	const ParameterBlock* right) {
	CHECK(left != nullptr);
	CHECK(right != nullptr);
	EXPECT_EQ(left->user_state(), right->user_state());
	EXPECT_EQ(left->Size(), right->Size());
	EXPECT_EQ(left->Size(), right->Size());
	EXPECT_EQ(left->LocalSize(), right->LocalSize());
	EXPECT_EQ(left->local_parameterization(), right->local_parameterization());
	EXPECT_EQ(left->IsConstant(), right->IsConstant());
	}

	TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) {
	// Parameter blocks with arbitrarily chosen initial values.
	double x[] = {1.0, 2.0, 3.0};
	double y[] = {4.0, 5.0, 6.0, 7.0};
	double z[] = {8.0, 9.0, 10.0, 11.0, 12.0};
	double w[] = {13.0, 14.0, 15.0, 16.0};

	ProblemImpl problem_impl;
	problem_impl.AddParameterBlock(x, 3);
	problem_impl.AddParameterBlock(y, 4);
	problem_impl.SetParameterBlockConstant(y);
	problem_impl.AddParameterBlock(z, 5);
	problem_impl.AddParameterBlock(w, 4, new QuaternionParameterization);
	problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
	problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4) ,
	NULL, z, y);
	problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5),
	new TrivialLoss, x, z);
	problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3),
	NULL, z, x);
	problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4),
	NULL, z, x, y);

	GradientCheckingIterationCallback callback;
	std::unique_ptr<ProblemImpl> gradient_checking_problem_impl(
	CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback));

	// The dimensions of the two problems match.
	EXPECT_EQ(problem_impl.NumParameterBlocks(),
	gradient_checking_problem_impl->NumParameterBlocks());
	EXPECT_EQ(problem_impl.NumResidualBlocks(),
	gradient_checking_problem_impl->NumResidualBlocks());

	EXPECT_EQ(problem_impl.NumParameters(),
	gradient_checking_problem_impl->NumParameters());
	EXPECT_EQ(problem_impl.NumResiduals(),
	gradient_checking_problem_impl->NumResiduals());

	const Program& program = problem_impl.program();
	const Program& gradient_checking_program =
	gradient_checking_problem_impl->program();

	// Since we added the ParameterBlocks and ResidualBlocks explicitly,
	// they should be in the same order in the two programs. It is
	// possible that may change due to implementation changes to
	// Program. This is not expected to be the case and writing code to
	// anticipate that possibility not worth the extra complexity in
	// this test.
	for (int i = 0; i < program.parameter_blocks().size(); ++i) {
	ParameterBlocksAreEquivalent(
	program.parameter_blocks()[i],
	gradient_checking_program.parameter_blocks()[i]);
	}

	for (int i = 0; i < program.residual_blocks().size(); ++i) {
	// Compare the sizes of the two ResidualBlocks.
	const ResidualBlock* original_residual_block =
	program.residual_blocks()[i];
	const ResidualBlock* new_residual_block =
	gradient_checking_program.residual_blocks()[i];
	EXPECT_EQ(original_residual_block->NumParameterBlocks(),
	new_residual_block->NumParameterBlocks());
	EXPECT_EQ(original_residual_block->NumResiduals(),
	new_residual_block->NumResiduals());
	EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(),
	new_residual_block->NumScratchDoublesForEvaluate());

	// Verify that the ParameterBlocks for the two residuals are equivalent.
	for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) {
	ParameterBlocksAreEquivalent(
	original_residual_block->parameter_blocks()[j],
	new_residual_block->parameter_blocks()[j]);
	}
	}
	}


	TEST(GradientCheckingProblemImpl, ConstrainedProblemBoundsArePropagated) {
	// Parameter blocks with arbitrarily chosen initial values.
	double x[] = {1.0, 2.0, 3.0};
	ProblemImpl problem_impl;
	problem_impl.AddParameterBlock(x, 3);
	problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
	problem_impl.SetParameterLowerBound(x,0,0.9);
	problem_impl.SetParameterUpperBound(x,1,2.5);

	GradientCheckingIterationCallback callback;
	std::unique_ptr<ProblemImpl> gradient_checking_problem_impl(
	CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback));

	// The dimensions of the two problems match.
	EXPECT_EQ(problem_impl.NumParameterBlocks(),
	gradient_checking_problem_impl->NumParameterBlocks());
	EXPECT_EQ(problem_impl.NumResidualBlocks(),
	gradient_checking_problem_impl->NumResidualBlocks());

	EXPECT_EQ(problem_impl.NumParameters(),
	gradient_checking_problem_impl->NumParameters());
	EXPECT_EQ(problem_impl.NumResiduals(),
	gradient_checking_problem_impl->NumResiduals());

	for (int i = 0; i < 3; ++i) {
	EXPECT_EQ(problem_impl.GetParameterLowerBound(x, i),
	gradient_checking_problem_impl->GetParameterLowerBound(x, i));
	EXPECT_EQ(problem_impl.GetParameterUpperBound(x, i),
	gradient_checking_problem_impl->GetParameterUpperBound(x, i));
	}
	}

	} // namespace internal
	} // namespace ceres