internal/ceres/levenberg_marquardt_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)
 //
 // This tests the Levenberg-Marquardt loop using a direct Evaluator
 // implementation, rather than having a test that goes through all the Program
 // and Problem machinery.

 #include <cmath>
 #include "ceres/dense_qr_solver.h"
 #include "ceres/dense_sparse_matrix.h"
 #include "ceres/evaluator.h"
 #include "ceres/levenberg_marquardt.h"
 #include "ceres/linear_solver.h"
 #include "ceres/minimizer.h"
 #include "ceres/internal/port.h"
 #include "gtest/gtest.h"

 namespace ceres {
 namespace internal {

 // Templated Evaluator for Powell's function. The template parameters
 // indicate which of the four variables/columns of the jacobian are
 // active. This is equivalent to constructing a problem and using the
 // SubsetLocalParameterization. This allows us to test the support for
 // the Evaluator::Plus operation besides checking for the basic
 // performance of the LevenbergMarquardt algorithm.
 template <bool col1, bool col2, bool col3, bool col4>
 class PowellEvaluator2 : public Evaluator {
  public:
   PowellEvaluator2()
       : num_active_cols_(
           (col1 ? 1 : 0) +
           (col2 ? 1 : 0) +
           (col3 ? 1 : 0) +
           (col4 ? 1 : 0)) {
     VLOG(1) << "Columns: "
             << col1 << " "
             << col2 << " "
             << col3 << " "
             << col4;
   }

   virtual ~PowellEvaluator2() {}

   // Implementation of Evaluator interface.
   virtual SparseMatrix* CreateJacobian() const {
     CHECK(col1 || col2 || col3 || col4);
     DenseSparseMatrix* dense_jacobian =
         new DenseSparseMatrix(NumResiduals(), NumEffectiveParameters());
     dense_jacobian->SetZero();
     return dense_jacobian;
   }

   virtual bool Evaluate(const double* state,
                         double* cost,
                         double* residuals,
                         SparseMatrix* jacobian) {
     double x1 = state[0];
     double x2 = state[1];
     double x3 = state[2];
     double x4 = state[3];

     VLOG(1) << "State: "
             << "x1=" << x1 << ", "
             << "x2=" << x2 << ", "
             << "x3=" << x3 << ", "
             << "x4=" << x4 << ".";

     double f1 = x1 + 10.0 * x2;
     double f2 = sqrt(5.0) * (x3 - x4);
     double f3 = pow(x2 - 2.0 * x3, 2.0);
     double f4 = sqrt(10.0) * pow(x1 - x4, 2.0);

     VLOG(1) << "Function: "
             << "f1=" << f1 << ", "
             << "f2=" << f2 << ", "
             << "f3=" << f3 << ", "
             << "f4=" << f4 << ".";

     *cost = (f1*f1 + f2*f2 + f3*f3 + f4*f4) / 2.0;

     VLOG(1) << "Cost: " << *cost;

     if (residuals != NULL) {
       residuals[0] = f1;
       residuals[1] = f2;
       residuals[2] = f3;
       residuals[3] = f4;
     }

     if (jacobian != NULL) {
       DenseSparseMatrix* dense_jacobian;
       dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian);
       dense_jacobian->SetZero();

       AlignedMatrixRef jacobian_matrix = dense_jacobian->mutable_matrix();
       CHECK_EQ(jacobian_matrix.cols(), num_active_cols_);

       int column_index = 0;
       if (col1) {
         jacobian_matrix.col(column_index++) <<
             1.0,
             0.0,
             0.0,
             sqrt(10) * 2.0 * (x1 - x4) * (1.0 - x4);
       }
       if (col2) {
         jacobian_matrix.col(column_index++) <<
             10.0,
             0.0,
             2.0*(x2 - 2.0*x3)*(1.0 - 2.0*x3),
             0.0;
       }

       if (col3) {
         jacobian_matrix.col(column_index++) <<
             0.0,
             sqrt(5.0),
             2.0*(x2 - 2.0*x3)*(x2 - 2.0),
             0.0;
       }

       if (col4) {
         jacobian_matrix.col(column_index++) <<
             0.0,
             -sqrt(5.0),
             0.0,
             sqrt(10) * 2.0 * (x1 - x4) * (x1 - 1.0);
       }
       VLOG(1) << "\n" << jacobian_matrix;
     }
     return true;
   }

   virtual bool Plus(const double* state,
                     const double* delta,
                     double* state_plus_delta) const {
     int delta_index = 0;
     state_plus_delta[0] = (col1  ? state[0] + delta[delta_index++] : state[0]);
     state_plus_delta[1] = (col2  ? state[1] + delta[delta_index++] : state[1]);
     state_plus_delta[2] = (col3  ? state[2] + delta[delta_index++] : state[2]);
     state_plus_delta[3] = (col4  ? state[3] + delta[delta_index++] : state[3]);
     return true;
   }

   virtual int NumEffectiveParameters() const { return num_active_cols_; }
   virtual int NumParameters()          const { return 4; }
   virtual int NumResiduals()           const { return 4; }

  private:
   const int num_active_cols_;
 };

 // Templated function to hold a subset of the columns fixed and check
 // if the solver converges to the optimal values or not.
 template<bool col1, bool col2, bool col3, bool col4>
 void IsSolveSuccessful() {
   LevenbergMarquardt lm;
   Solver::Options solver_options;
   Minimizer::Options minimizer_options(solver_options);
   minimizer_options.gradient_tolerance = 1e-26;
   minimizer_options.function_tolerance = 1e-26;
   minimizer_options.parameter_tolerance = 1e-26;
   LinearSolver::Options linear_solver_options;
   DenseQRSolver linear_solver(linear_solver_options);

   double initial_parameters[4] = { 3, -1, 0, 1.0 };
   double final_parameters[4] = { -1.0, -1.0, -1.0, -1.0 };

   // If the column is inactive, then set its value to the optimal
   // value.
   initial_parameters[0] = (col1 ? initial_parameters[0] : 0.0);
   initial_parameters[1] = (col2 ? initial_parameters[1] : 0.0);
   initial_parameters[2] = (col3 ? initial_parameters[2] : 0.0);
   initial_parameters[3] = (col4 ? initial_parameters[3] : 0.0);

   PowellEvaluator2<col1, col2, col3, col4> powell_evaluator;

   Solver::Summary summary;
   lm.Minimize(minimizer_options,
               &powell_evaluator,
               &linear_solver,
               initial_parameters,
               final_parameters,
               &summary);

   // The minimum is at x1 = x2 = x3 = x4 = 0.
   EXPECT_NEAR(0.0, final_parameters[0], 0.001);
   EXPECT_NEAR(0.0, final_parameters[1], 0.001);
   EXPECT_NEAR(0.0, final_parameters[2], 0.001);
   EXPECT_NEAR(0.0, final_parameters[3], 0.001);
 };

 TEST(LevenbergMarquardt, PowellsSingularFunction) {
   // This case is excluded because this has a local minimum and does
   // not find the optimum. This should not affect the correctness of
   // this test since we are testing all the other 14 combinations of
   // column activations.

   // IsSolveSuccessful<true, true, false, true>();

   IsSolveSuccessful<true,  true,  true,  true>();
   IsSolveSuccessful<true,  true,  true,  false>();
   IsSolveSuccessful<true,  false, true,  true>();
   IsSolveSuccessful<false, true,  true,  true>();
   IsSolveSuccessful<true,  true,  false, false>();
   IsSolveSuccessful<true,  false, true,  false>();
   IsSolveSuccessful<false, true,  true,  false>();
   IsSolveSuccessful<true,  false, false, true>();
   IsSolveSuccessful<false, true,  false, true>();
   IsSolveSuccessful<false, false, true,  true>();
   IsSolveSuccessful<true,  false, false, false>();
   IsSolveSuccessful<false, true,  false, false>();
   IsSolveSuccessful<false, false, true,  false>();
   IsSolveSuccessful<false, false, false, true>();
 }


 }  // 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)
	//
	// This tests the Levenberg-Marquardt loop using a direct Evaluator
	// implementation, rather than having a test that goes through all the Program
	// and Problem machinery.

	#include <cmath>
	#include "ceres/dense_qr_solver.h"
	#include "ceres/dense_sparse_matrix.h"
	#include "ceres/evaluator.h"
	#include "ceres/levenberg_marquardt.h"
	#include "ceres/linear_solver.h"
	#include "ceres/minimizer.h"
	#include "ceres/internal/port.h"
	#include "gtest/gtest.h"

	namespace ceres {
	namespace internal {

	// Templated Evaluator for Powell's function. The template parameters
	// indicate which of the four variables/columns of the jacobian are
	// active. This is equivalent to constructing a problem and using the
	// SubsetLocalParameterization. This allows us to test the support for
	// the Evaluator::Plus operation besides checking for the basic
	// performance of the LevenbergMarquardt algorithm.
	template <bool col1, bool col2, bool col3, bool col4>
	class PowellEvaluator2 : public Evaluator {
	public:
	PowellEvaluator2()
	: num_active_cols_(
	(col1 ? 1 : 0) +
	(col2 ? 1 : 0) +
	(col3 ? 1 : 0) +
	(col4 ? 1 : 0)) {
	VLOG(1) << "Columns: "
	<< col1 << " "
	<< col2 << " "
	<< col3 << " "
	<< col4;
	}

	virtual ~PowellEvaluator2() {}

	// Implementation of Evaluator interface.
	virtual SparseMatrix* CreateJacobian() const {
	CHECK(col1 \|\| col2 \|\| col3 \|\| col4);
	DenseSparseMatrix* dense_jacobian =
	new DenseSparseMatrix(NumResiduals(), NumEffectiveParameters());
	dense_jacobian->SetZero();
	return dense_jacobian;
	}

	virtual bool Evaluate(const double* state,
	double* cost,
	double* residuals,
	SparseMatrix* jacobian) {
	double x1 = state[0];
	double x2 = state[1];
	double x3 = state[2];
	double x4 = state[3];

	VLOG(1) << "State: "
	<< "x1=" << x1 << ", "
	<< "x2=" << x2 << ", "
	<< "x3=" << x3 << ", "
	<< "x4=" << x4 << ".";

	double f1 = x1 + 10.0 * x2;
	double f2 = sqrt(5.0) * (x3 - x4);
	double f3 = pow(x2 - 2.0 * x3, 2.0);
	double f4 = sqrt(10.0) * pow(x1 - x4, 2.0);

	VLOG(1) << "Function: "
	<< "f1=" << f1 << ", "
	<< "f2=" << f2 << ", "
	<< "f3=" << f3 << ", "
	<< "f4=" << f4 << ".";

	cost = (f1f1 + f2f2 + f3f3 + f4*f4) / 2.0;

	VLOG(1) << "Cost: " << *cost;

	if (residuals != NULL) {
	residuals[0] = f1;
	residuals[1] = f2;
	residuals[2] = f3;
	residuals[3] = f4;
	}

	if (jacobian != NULL) {
	DenseSparseMatrix* dense_jacobian;
	dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian);
	dense_jacobian->SetZero();

	AlignedMatrixRef jacobian_matrix = dense_jacobian->mutable_matrix();
	CHECK_EQ(jacobian_matrix.cols(), num_active_cols_);

	int column_index = 0;
	if (col1) {
	jacobian_matrix.col(column_index++) <<
	1.0,
	0.0,
	0.0,
	sqrt(10) * 2.0 * (x1 - x4) * (1.0 - x4);
	}
	if (col2) {
	jacobian_matrix.col(column_index++) <<
	10.0,
	0.0,
	2.0(x2 - 2.0x3)(1.0 - 2.0x3),
	0.0;
	}

	if (col3) {
	jacobian_matrix.col(column_index++) <<
	0.0,
	sqrt(5.0),
	2.0(x2 - 2.0x3)*(x2 - 2.0),
	0.0;
	}

	if (col4) {
	jacobian_matrix.col(column_index++) <<
	0.0,
	-sqrt(5.0),
	0.0,
	sqrt(10) * 2.0 * (x1 - x4) * (x1 - 1.0);
	}
	VLOG(1) << "\n" << jacobian_matrix;
	}
	return true;
	}

	virtual bool Plus(const double* state,
	const double* delta,
	double* state_plus_delta) const {
	int delta_index = 0;
	state_plus_delta[0] = (col1 ? state[0] + delta[delta_index++] : state[0]);
	state_plus_delta[1] = (col2 ? state[1] + delta[delta_index++] : state[1]);
	state_plus_delta[2] = (col3 ? state[2] + delta[delta_index++] : state[2]);
	state_plus_delta[3] = (col4 ? state[3] + delta[delta_index++] : state[3]);
	return true;
	}

	virtual int NumEffectiveParameters() const { return num_active_cols_; }
	virtual int NumParameters() const { return 4; }
	virtual int NumResiduals() const { return 4; }

	private:
	const int num_active_cols_;
	};

	// Templated function to hold a subset of the columns fixed and check
	// if the solver converges to the optimal values or not.
	template<bool col1, bool col2, bool col3, bool col4>
	void IsSolveSuccessful() {
	LevenbergMarquardt lm;
	Solver::Options solver_options;
	Minimizer::Options minimizer_options(solver_options);
	minimizer_options.gradient_tolerance = 1e-26;
	minimizer_options.function_tolerance = 1e-26;
	minimizer_options.parameter_tolerance = 1e-26;
	LinearSolver::Options linear_solver_options;
	DenseQRSolver linear_solver(linear_solver_options);

	double initial_parameters[4] = { 3, -1, 0, 1.0 };
	double final_parameters[4] = { -1.0, -1.0, -1.0, -1.0 };

	// If the column is inactive, then set its value to the optimal
	// value.
	initial_parameters[0] = (col1 ? initial_parameters[0] : 0.0);
	initial_parameters[1] = (col2 ? initial_parameters[1] : 0.0);
	initial_parameters[2] = (col3 ? initial_parameters[2] : 0.0);
	initial_parameters[3] = (col4 ? initial_parameters[3] : 0.0);

	PowellEvaluator2<col1, col2, col3, col4> powell_evaluator;

	Solver::Summary summary;
	lm.Minimize(minimizer_options,
	&powell_evaluator,
	&linear_solver,
	initial_parameters,
	final_parameters,
	&summary);

	// The minimum is at x1 = x2 = x3 = x4 = 0.
	EXPECT_NEAR(0.0, final_parameters[0], 0.001);
	EXPECT_NEAR(0.0, final_parameters[1], 0.001);
	EXPECT_NEAR(0.0, final_parameters[2], 0.001);
	EXPECT_NEAR(0.0, final_parameters[3], 0.001);
	};

	TEST(LevenbergMarquardt, PowellsSingularFunction) {
	// This case is excluded because this has a local minimum and does
	// not find the optimum. This should not affect the correctness of
	// this test since we are testing all the other 14 combinations of
	// column activations.

	// IsSolveSuccessful<true, true, false, true>();

	IsSolveSuccessful<true, true, true, true>();
	IsSolveSuccessful<true, true, true, false>();
	IsSolveSuccessful<true, false, true, true>();
	IsSolveSuccessful<false, true, true, true>();
	IsSolveSuccessful<true, true, false, false>();
	IsSolveSuccessful<true, false, true, false>();
	IsSolveSuccessful<false, true, true, false>();
	IsSolveSuccessful<true, false, false, true>();
	IsSolveSuccessful<false, true, false, true>();
	IsSolveSuccessful<false, false, true, true>();
	IsSolveSuccessful<true, false, false, false>();
	IsSolveSuccessful<false, true, false, false>();
	IsSolveSuccessful<false, false, true, false>();
	IsSolveSuccessful<false, false, false, true>();
	}


	} // namespace internal
	} // namespace ceres