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

 // Ceres Solver - A fast non-linear least squares minimizer
 // Copyright 2018 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: sameeragarwal@google.com (Sameer Agarwal)

 #include "ceres/iterative_refiner.h"

 #include <utility>

 #include "Eigen/Dense"
 #include "ceres/dense_cholesky.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/sparse_cholesky.h"
 #include "ceres/sparse_matrix.h"
 #include "glog/logging.h"
 #include "gtest/gtest.h"

 namespace ceres::internal {

 // Macros to help us define virtual methods which we do not expect to
 // use/call in this test.
 #define DO_NOT_CALL \
   { LOG(FATAL) << "DO NOT CALL"; }
 #define DO_NOT_CALL_WITH_RETURN(x) \
   {                                \
     LOG(FATAL) << "DO NOT CALL";   \
     return x;                      \
   }

 // A fake SparseMatrix, which uses an Eigen matrix to do the real work.
 class FakeSparseMatrix : public SparseMatrix {
  public:
   explicit FakeSparseMatrix(Matrix m) : m_(std::move(m)) {}

   // y += Ax
   void RightMultiplyAndAccumulate(const double* x, double* y) const final {
     VectorRef(y, m_.cols()) += m_ * ConstVectorRef(x, m_.cols());
   }
   // y += A'x
   void LeftMultiplyAndAccumulate(const double* x, double* y) const final {
     // We will assume that this is a symmetric matrix.
     RightMultiplyAndAccumulate(x, y);
   }

   double* mutable_values() final { return m_.data(); }
   const double* values() const final { return m_.data(); }
   int num_rows() const final { return m_.cols(); }
   int num_cols() const final { return m_.cols(); }
   int num_nonzeros() const final { return m_.cols() * m_.cols(); }

   // The following methods are not needed for tests in this file.
   void SquaredColumnNorm(double* x) const final DO_NOT_CALL;
   void ScaleColumns(const double* scale) final DO_NOT_CALL;
   void SetZero() final DO_NOT_CALL;
   void ToDenseMatrix(Matrix* dense_matrix) const final DO_NOT_CALL;
   void ToTextFile(FILE* file) const final DO_NOT_CALL;

  private:
   Matrix m_;
 };

 // A fake SparseCholesky which uses Eigen's Cholesky factorization to
 // do the real work. The template parameter allows us to work in
 // doubles or floats, even though the source matrix is double.
 template <typename Scalar>
 class FakeSparseCholesky : public SparseCholesky {
  public:
   explicit FakeSparseCholesky(const Matrix& lhs) { lhs_ = lhs.cast<Scalar>(); }

   LinearSolverTerminationType Solve(const double* rhs_ptr,
                                     double* solution_ptr,
                                     std::string* message) final {
     const int num_cols = lhs_.cols();
     VectorRef solution(solution_ptr, num_cols);
     ConstVectorRef rhs(rhs_ptr, num_cols);
     auto llt = lhs_.llt();
     CHECK_EQ(llt.info(), Eigen::Success);
     solution = llt.solve(rhs.cast<Scalar>()).template cast<double>();
     return LinearSolverTerminationType::SUCCESS;
   }

   // The following methods are not needed for tests in this file.
   CompressedRowSparseMatrix::StorageType StorageType() const final
       DO_NOT_CALL_WITH_RETURN(
           CompressedRowSparseMatrix::StorageType::UPPER_TRIANGULAR);
   LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs,
                                         std::string* message) final
       DO_NOT_CALL_WITH_RETURN(LinearSolverTerminationType::FAILURE);

  private:
   Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> lhs_;
 };

 // A fake DenseCholesky which uses Eigen's Cholesky factorization to
 // do the real work. The template parameter allows us to work in
 // doubles or floats, even though the source matrix is double.
 template <typename Scalar>
 class FakeDenseCholesky : public DenseCholesky {
  public:
   explicit FakeDenseCholesky(const Matrix& lhs) { lhs_ = lhs.cast<Scalar>(); }

   LinearSolverTerminationType Solve(const double* rhs_ptr,
                                     double* solution_ptr,
                                     std::string* message) final {
     const int num_cols = lhs_.cols();
     VectorRef solution(solution_ptr, num_cols);
     ConstVectorRef rhs(rhs_ptr, num_cols);
     solution = lhs_.llt().solve(rhs.cast<Scalar>()).template cast<double>();
     return LinearSolverTerminationType::SUCCESS;
   }

   LinearSolverTerminationType Factorize(int num_cols,
                                         double* lhs,
                                         std::string* message) final
       DO_NOT_CALL_WITH_RETURN(LinearSolverTerminationType::FAILURE);

  private:
   Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> lhs_;
 };

 #undef DO_NOT_CALL
 #undef DO_NOT_CALL_WITH_RETURN

 class SparseIterativeRefinerTest : public ::testing::Test {
  public:
   void SetUp() override {
     num_cols_ = 5;
     max_num_iterations_ = 30;
     Matrix m(num_cols_, num_cols_);
     m.setRandom();
     lhs_ = m * m.transpose();
     solution_.resize(num_cols_);
     solution_.setRandom();
     rhs_ = lhs_ * solution_;
   };

  protected:
   int num_cols_;
   int max_num_iterations_;
   Matrix lhs_;
   Vector rhs_, solution_;
 };

 TEST_F(SparseIterativeRefinerTest,
        RandomSolutionWithExactFactorizationConverges) {
   FakeSparseMatrix lhs(lhs_);
   FakeSparseCholesky<double> sparse_cholesky(lhs_);
   SparseIterativeRefiner refiner(max_num_iterations_);
   Vector refined_solution(num_cols_);
   refined_solution.setRandom();
   refiner.Refine(lhs, rhs_.data(), &sparse_cholesky, refined_solution.data());
   EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
               0.0,
               std::numeric_limits<double>::epsilon() * 10);
 }

 TEST_F(SparseIterativeRefinerTest,
        RandomSolutionWithApproximationFactorizationConverges) {
   FakeSparseMatrix lhs(lhs_);
   // Use a single precision Cholesky factorization of the double
   // precision matrix. This will give us an approximate factorization.
   FakeSparseCholesky<float> sparse_cholesky(lhs_);
   SparseIterativeRefiner refiner(max_num_iterations_);
   Vector refined_solution(num_cols_);
   refined_solution.setRandom();
   refiner.Refine(lhs, rhs_.data(), &sparse_cholesky, refined_solution.data());
   EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
               0.0,
               std::numeric_limits<double>::epsilon() * 10);
 }

 class DenseIterativeRefinerTest : public ::testing::Test {
  public:
   void SetUp() override {
     num_cols_ = 5;
     max_num_iterations_ = 30;
     Matrix m(num_cols_, num_cols_);
     m.setRandom();
     lhs_ = m * m.transpose();
     solution_.resize(num_cols_);
     solution_.setRandom();
     rhs_ = lhs_ * solution_;
   };

  protected:
   int num_cols_;
   int max_num_iterations_;
   Matrix lhs_;
   Vector rhs_, solution_;
 };

 TEST_F(DenseIterativeRefinerTest,
        RandomSolutionWithExactFactorizationConverges) {
   Matrix lhs = lhs_;
   FakeDenseCholesky<double> dense_cholesky(lhs);
   DenseIterativeRefiner refiner(max_num_iterations_);
   Vector refined_solution(num_cols_);
   refined_solution.setRandom();
   refiner.Refine(lhs.cols(),
                  lhs.data(),
                  rhs_.data(),
                  &dense_cholesky,
                  refined_solution.data());
   EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
               0.0,
               std::numeric_limits<double>::epsilon() * 10);
 }

 TEST_F(DenseIterativeRefinerTest,
        RandomSolutionWithApproximationFactorizationConverges) {
   Matrix lhs = lhs_;
   // Use a single precision Cholesky factorization of the double
   // precision matrix. This will give us an approximate factorization.
   FakeDenseCholesky<float> dense_cholesky(lhs_);
   DenseIterativeRefiner refiner(max_num_iterations_);
   Vector refined_solution(num_cols_);
   refined_solution.setRandom();
   refiner.Refine(lhs.cols(),
                  lhs.data(),
                  rhs_.data(),
                  &dense_cholesky,
                  refined_solution.data());
   EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
               0.0,
               std::numeric_limits<double>::epsilon() * 10);
 }

 }  // namespace ceres::internal
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2018 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: sameeragarwal@google.com (Sameer Agarwal)

	#include "ceres/iterative_refiner.h"

	#include <utility>

	#include "Eigen/Dense"
	#include "ceres/dense_cholesky.h"
	#include "ceres/internal/eigen.h"
	#include "ceres/sparse_cholesky.h"
	#include "ceres/sparse_matrix.h"
	#include "glog/logging.h"
	#include "gtest/gtest.h"

	namespace ceres::internal {

	// Macros to help us define virtual methods which we do not expect to
	// use/call in this test.
	#define DO_NOT_CALL \
	{ LOG(FATAL) << "DO NOT CALL"; }
	#define DO_NOT_CALL_WITH_RETURN(x) \
	{ \
	LOG(FATAL) << "DO NOT CALL"; \
	return x; \
	}

	// A fake SparseMatrix, which uses an Eigen matrix to do the real work.
	class FakeSparseMatrix : public SparseMatrix {
	public:
	explicit FakeSparseMatrix(Matrix m) : m_(std::move(m)) {}

	// y += Ax
	void RightMultiplyAndAccumulate(const double* x, double* y) const final {
	VectorRef(y, m_.cols()) += m_ * ConstVectorRef(x, m_.cols());
	}
	// y += A'x
	void LeftMultiplyAndAccumulate(const double* x, double* y) const final {
	// We will assume that this is a symmetric matrix.
	RightMultiplyAndAccumulate(x, y);
	}

	double* mutable_values() final { return m_.data(); }
	const double* values() const final { return m_.data(); }
	int num_rows() const final { return m_.cols(); }
	int num_cols() const final { return m_.cols(); }
	int num_nonzeros() const final { return m_.cols() * m_.cols(); }

	// The following methods are not needed for tests in this file.
	void SquaredColumnNorm(double* x) const final DO_NOT_CALL;
	void ScaleColumns(const double* scale) final DO_NOT_CALL;
	void SetZero() final DO_NOT_CALL;
	void ToDenseMatrix(Matrix* dense_matrix) const final DO_NOT_CALL;
	void ToTextFile(FILE* file) const final DO_NOT_CALL;

	private:
	Matrix m_;
	};

	// A fake SparseCholesky which uses Eigen's Cholesky factorization to
	// do the real work. The template parameter allows us to work in
	// doubles or floats, even though the source matrix is double.
	template <typename Scalar>
	class FakeSparseCholesky : public SparseCholesky {
	public:
	explicit FakeSparseCholesky(const Matrix& lhs) { lhs_ = lhs.cast<Scalar>(); }

	LinearSolverTerminationType Solve(const double* rhs_ptr,
	double* solution_ptr,
	std::string* message) final {
	const int num_cols = lhs_.cols();
	VectorRef solution(solution_ptr, num_cols);
	ConstVectorRef rhs(rhs_ptr, num_cols);
	auto llt = lhs_.llt();
	CHECK_EQ(llt.info(), Eigen::Success);
	solution = llt.solve(rhs.cast<Scalar>()).template cast<double>();
	return LinearSolverTerminationType::SUCCESS;
	}

	// The following methods are not needed for tests in this file.
	CompressedRowSparseMatrix::StorageType StorageType() const final
	DO_NOT_CALL_WITH_RETURN(
	CompressedRowSparseMatrix::StorageType::UPPER_TRIANGULAR);
	LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs,
	std::string* message) final
	DO_NOT_CALL_WITH_RETURN(LinearSolverTerminationType::FAILURE);

	private:
	Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> lhs_;
	};

	// A fake DenseCholesky which uses Eigen's Cholesky factorization to
	// do the real work. The template parameter allows us to work in
	// doubles or floats, even though the source matrix is double.
	template <typename Scalar>
	class FakeDenseCholesky : public DenseCholesky {
	public:
	explicit FakeDenseCholesky(const Matrix& lhs) { lhs_ = lhs.cast<Scalar>(); }

	LinearSolverTerminationType Solve(const double* rhs_ptr,
	double* solution_ptr,
	std::string* message) final {
	const int num_cols = lhs_.cols();
	VectorRef solution(solution_ptr, num_cols);
	ConstVectorRef rhs(rhs_ptr, num_cols);
	solution = lhs_.llt().solve(rhs.cast<Scalar>()).template cast<double>();
	return LinearSolverTerminationType::SUCCESS;
	}

	LinearSolverTerminationType Factorize(int num_cols,
	double* lhs,
	std::string* message) final
	DO_NOT_CALL_WITH_RETURN(LinearSolverTerminationType::FAILURE);

	private:
	Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> lhs_;
	};

	#undef DO_NOT_CALL
	#undef DO_NOT_CALL_WITH_RETURN

	class SparseIterativeRefinerTest : public ::testing::Test {
	public:
	void SetUp() override {
	num_cols_ = 5;
	max_num_iterations_ = 30;
	Matrix m(num_cols_, num_cols_);
	m.setRandom();
	lhs_ = m * m.transpose();
	solution_.resize(num_cols_);
	solution_.setRandom();
	rhs_ = lhs_ * solution_;
	};

	protected:
	int num_cols_;
	int max_num_iterations_;
	Matrix lhs_;
	Vector rhs_, solution_;
	};

	TEST_F(SparseIterativeRefinerTest,
	RandomSolutionWithExactFactorizationConverges) {
	FakeSparseMatrix lhs(lhs_);
	FakeSparseCholesky<double> sparse_cholesky(lhs_);
	SparseIterativeRefiner refiner(max_num_iterations_);
	Vector refined_solution(num_cols_);
	refined_solution.setRandom();
	refiner.Refine(lhs, rhs_.data(), &sparse_cholesky, refined_solution.data());
	EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
	0.0,
	std::numeric_limits<double>::epsilon() * 10);
	}

	TEST_F(SparseIterativeRefinerTest,
	RandomSolutionWithApproximationFactorizationConverges) {
	FakeSparseMatrix lhs(lhs_);
	// Use a single precision Cholesky factorization of the double
	// precision matrix. This will give us an approximate factorization.
	FakeSparseCholesky<float> sparse_cholesky(lhs_);
	SparseIterativeRefiner refiner(max_num_iterations_);
	Vector refined_solution(num_cols_);
	refined_solution.setRandom();
	refiner.Refine(lhs, rhs_.data(), &sparse_cholesky, refined_solution.data());
	EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
	0.0,
	std::numeric_limits<double>::epsilon() * 10);
	}

	class DenseIterativeRefinerTest : public ::testing::Test {
	public:
	void SetUp() override {
	num_cols_ = 5;
	max_num_iterations_ = 30;
	Matrix m(num_cols_, num_cols_);
	m.setRandom();
	lhs_ = m * m.transpose();
	solution_.resize(num_cols_);
	solution_.setRandom();
	rhs_ = lhs_ * solution_;
	};

	protected:
	int num_cols_;
	int max_num_iterations_;
	Matrix lhs_;
	Vector rhs_, solution_;
	};

	TEST_F(DenseIterativeRefinerTest,
	RandomSolutionWithExactFactorizationConverges) {
	Matrix lhs = lhs_;
	FakeDenseCholesky<double> dense_cholesky(lhs);
	DenseIterativeRefiner refiner(max_num_iterations_);
	Vector refined_solution(num_cols_);
	refined_solution.setRandom();
	refiner.Refine(lhs.cols(),
	lhs.data(),
	rhs_.data(),
	&dense_cholesky,
	refined_solution.data());
	EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
	0.0,
	std::numeric_limits<double>::epsilon() * 10);
	}

	TEST_F(DenseIterativeRefinerTest,
	RandomSolutionWithApproximationFactorizationConverges) {
	Matrix lhs = lhs_;
	// Use a single precision Cholesky factorization of the double
	// precision matrix. This will give us an approximate factorization.
	FakeDenseCholesky<float> dense_cholesky(lhs_);
	DenseIterativeRefiner refiner(max_num_iterations_);
	Vector refined_solution(num_cols_);
	refined_solution.setRandom();
	refiner.Refine(lhs.cols(),
	lhs.data(),
	rhs_.data(),
	&dense_cholesky,
	refined_solution.data());
	EXPECT_NEAR((lhs_ * refined_solution - rhs_).norm(),
	0.0,
	std::numeric_limits<double>::epsilon() * 10);
	}

	} // namespace ceres::internal