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

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

 #include <algorithm>
 #include <memory>
 #include <random>
 #include <vector>

 #include "Eigen/Dense"
 #include "benchmark/benchmark.h"
 #include "ceres/block_random_access_dense_matrix.h"
 #include "ceres/block_sparse_matrix.h"
 #include "ceres/block_structure.h"
 #include "ceres/schur_eliminator.h"

 namespace ceres::internal {

 constexpr int kRowBlockSize = 2;
 constexpr int kEBlockSize = 3;
 constexpr int kFBlockSize = 6;

 class BenchmarkData {
  public:
   explicit BenchmarkData(const int num_e_blocks) {
     auto* bs = new CompressedRowBlockStructure;
     bs->cols.resize(num_e_blocks + 1);
     int col_pos = 0;
     for (int i = 0; i < num_e_blocks; ++i) {
       bs->cols[i].position = col_pos;
       bs->cols[i].size = kEBlockSize;
       col_pos += kEBlockSize;
     }
     bs->cols.back().position = col_pos;
     bs->cols.back().size = kFBlockSize;

     bs->rows.resize(2 * num_e_blocks);
     int row_pos = 0;
     int cell_pos = 0;
     for (int i = 0; i < num_e_blocks; ++i) {
       {
         auto& row = bs->rows[2 * i];
         row.block.position = row_pos;
         row.block.size = kRowBlockSize;
         row_pos += kRowBlockSize;
         auto& cells = row.cells;
         cells.resize(2);
         cells[0].block_id = i;
         cells[0].position = cell_pos;
         cell_pos += kRowBlockSize * kEBlockSize;
         cells[1].block_id = num_e_blocks;
         cells[1].position = cell_pos;
         cell_pos += kRowBlockSize * kFBlockSize;
       }
       {
         auto& row = bs->rows[2 * i + 1];
         row.block.position = row_pos;
         row.block.size = kRowBlockSize;
         row_pos += kRowBlockSize;
         auto& cells = row.cells;
         cells.resize(1);
         cells[0].block_id = i;
         cells[0].position = cell_pos;
         cell_pos += kRowBlockSize * kEBlockSize;
       }
     }

     matrix_ = std::make_unique<BlockSparseMatrix>(bs);
     double* values = matrix_->mutable_values();
     std::generate_n(values, matrix_->num_nonzeros(), [this] {
       return standard_normal_(prng_);
     });

     b_.resize(matrix_->num_rows());
     b_.setRandom();

     std::vector<Block> blocks;
     blocks.emplace_back(kFBlockSize, 0);
     lhs_ = std::make_unique<BlockRandomAccessDenseMatrix>(blocks, &context_, 1);
     diagonal_.resize(matrix_->num_cols());
     diagonal_.setOnes();
     rhs_.resize(kFBlockSize);

     y_.resize(num_e_blocks * kEBlockSize);
     y_.setZero();
     z_.resize(kFBlockSize);
     z_.setOnes();
   }

   const BlockSparseMatrix& matrix() const { return *matrix_; }
   const Vector& b() const { return b_; }
   const Vector& diagonal() const { return diagonal_; }
   BlockRandomAccessDenseMatrix* mutable_lhs() { return lhs_.get(); }
   Vector* mutable_rhs() { return &rhs_; }
   Vector* mutable_y() { return &y_; }
   Vector* mutable_z() { return &z_; }

   ContextImpl* context() { return &context_; }

  private:
   ContextImpl context_;

   std::unique_ptr<BlockSparseMatrix> matrix_;
   Vector b_;
   std::unique_ptr<BlockRandomAccessDenseMatrix> lhs_;
   Vector rhs_;
   Vector diagonal_;
   Vector z_;
   Vector y_;
   std::mt19937 prng_;
   std::normal_distribution<> standard_normal_;
 };

 static void BM_SchurEliminatorEliminate(benchmark::State& state) {
   const int num_e_blocks = state.range(0);
   BenchmarkData data(num_e_blocks);

   LinearSolver::Options linear_solver_options;
   linear_solver_options.e_block_size = kEBlockSize;
   linear_solver_options.row_block_size = kRowBlockSize;
   linear_solver_options.f_block_size = kFBlockSize;
   linear_solver_options.context = data.context();
   std::unique_ptr<SchurEliminatorBase> eliminator(
       SchurEliminatorBase::Create(linear_solver_options));

   eliminator->Init(num_e_blocks, true, data.matrix().block_structure());
   for (auto _ : state) {
     eliminator->Eliminate(BlockSparseMatrixData(data.matrix()),
                           data.b().data(),
                           data.diagonal().data(),
                           data.mutable_lhs(),
                           data.mutable_rhs()->data());
   }
 }

 static void BM_SchurEliminatorBackSubstitute(benchmark::State& state) {
   const int num_e_blocks = state.range(0);
   BenchmarkData data(num_e_blocks);

   LinearSolver::Options linear_solver_options;
   linear_solver_options.e_block_size = kEBlockSize;
   linear_solver_options.row_block_size = kRowBlockSize;
   linear_solver_options.f_block_size = kFBlockSize;
   linear_solver_options.context = data.context();
   std::unique_ptr<SchurEliminatorBase> eliminator(
       SchurEliminatorBase::Create(linear_solver_options));

   eliminator->Init(num_e_blocks, true, data.matrix().block_structure());
   eliminator->Eliminate(BlockSparseMatrixData(data.matrix()),
                         data.b().data(),
                         data.diagonal().data(),
                         data.mutable_lhs(),
                         data.mutable_rhs()->data());
   for (auto _ : state) {
     eliminator->BackSubstitute(BlockSparseMatrixData(data.matrix()),
                                data.b().data(),
                                data.diagonal().data(),
                                data.mutable_z()->data(),
                                data.mutable_y()->data());
   }
 }

 static void BM_SchurEliminatorForOneFBlockEliminate(benchmark::State& state) {
   const int num_e_blocks = state.range(0);
   BenchmarkData data(num_e_blocks);
   SchurEliminatorForOneFBlock<2, 3, 6> eliminator;
   eliminator.Init(num_e_blocks, true, data.matrix().block_structure());
   for (auto _ : state) {
     eliminator.Eliminate(BlockSparseMatrixData(data.matrix()),
                          data.b().data(),
                          data.diagonal().data(),
                          data.mutable_lhs(),
                          data.mutable_rhs()->data());
   }
 }

 static void BM_SchurEliminatorForOneFBlockBackSubstitute(
     benchmark::State& state) {
   const int num_e_blocks = state.range(0);
   BenchmarkData data(num_e_blocks);
   SchurEliminatorForOneFBlock<2, 3, 6> eliminator;
   eliminator.Init(num_e_blocks, true, data.matrix().block_structure());
   eliminator.Eliminate(BlockSparseMatrixData(data.matrix()),
                        data.b().data(),
                        data.diagonal().data(),
                        data.mutable_lhs(),
                        data.mutable_rhs()->data());
   for (auto _ : state) {
     eliminator.BackSubstitute(BlockSparseMatrixData(data.matrix()),
                               data.b().data(),
                               data.diagonal().data(),
                               data.mutable_z()->data(),
                               data.mutable_y()->data());
   }
 }

 BENCHMARK(BM_SchurEliminatorEliminate)->Range(10, 10000);
 BENCHMARK(BM_SchurEliminatorForOneFBlockEliminate)->Range(10, 10000);
 BENCHMARK(BM_SchurEliminatorBackSubstitute)->Range(10, 10000);
 BENCHMARK(BM_SchurEliminatorForOneFBlockBackSubstitute)->Range(10, 10000);

 }  // namespace ceres::internal

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

	#include <algorithm>
	#include <memory>
	#include <random>
	#include <vector>

	#include "Eigen/Dense"
	#include "benchmark/benchmark.h"
	#include "ceres/block_random_access_dense_matrix.h"
	#include "ceres/block_sparse_matrix.h"
	#include "ceres/block_structure.h"
	#include "ceres/schur_eliminator.h"

	namespace ceres::internal {

	constexpr int kRowBlockSize = 2;
	constexpr int kEBlockSize = 3;
	constexpr int kFBlockSize = 6;

	class BenchmarkData {
	public:
	explicit BenchmarkData(const int num_e_blocks) {
	auto* bs = new CompressedRowBlockStructure;
	bs->cols.resize(num_e_blocks + 1);
	int col_pos = 0;
	for (int i = 0; i < num_e_blocks; ++i) {
	bs->cols[i].position = col_pos;
	bs->cols[i].size = kEBlockSize;
	col_pos += kEBlockSize;
	}
	bs->cols.back().position = col_pos;
	bs->cols.back().size = kFBlockSize;

	bs->rows.resize(2 * num_e_blocks);
	int row_pos = 0;
	int cell_pos = 0;
	for (int i = 0; i < num_e_blocks; ++i) {
	{
	auto& row = bs->rows[2 * i];
	row.block.position = row_pos;
	row.block.size = kRowBlockSize;
	row_pos += kRowBlockSize;
	auto& cells = row.cells;
	cells.resize(2);
	cells[0].block_id = i;
	cells[0].position = cell_pos;
	cell_pos += kRowBlockSize * kEBlockSize;
	cells[1].block_id = num_e_blocks;
	cells[1].position = cell_pos;
	cell_pos += kRowBlockSize * kFBlockSize;
	}
	{
	auto& row = bs->rows[2 * i + 1];
	row.block.position = row_pos;
	row.block.size = kRowBlockSize;
	row_pos += kRowBlockSize;
	auto& cells = row.cells;
	cells.resize(1);
	cells[0].block_id = i;
	cells[0].position = cell_pos;
	cell_pos += kRowBlockSize * kEBlockSize;
	}
	}

	matrix_ = std::make_unique<BlockSparseMatrix>(bs);
	double* values = matrix_->mutable_values();
	std::generate_n(values, matrix_->num_nonzeros(), [this] {
	return standard_normal_(prng_);
	});

	b_.resize(matrix_->num_rows());
	b_.setRandom();

	std::vector<Block> blocks;
	blocks.emplace_back(kFBlockSize, 0);
	lhs_ = std::make_unique<BlockRandomAccessDenseMatrix>(blocks, &context_, 1);
	diagonal_.resize(matrix_->num_cols());
	diagonal_.setOnes();
	rhs_.resize(kFBlockSize);

	y_.resize(num_e_blocks * kEBlockSize);
	y_.setZero();
	z_.resize(kFBlockSize);
	z_.setOnes();
	}

	const BlockSparseMatrix& matrix() const { return *matrix_; }
	const Vector& b() const { return b_; }
	const Vector& diagonal() const { return diagonal_; }
	BlockRandomAccessDenseMatrix* mutable_lhs() { return lhs_.get(); }
	Vector* mutable_rhs() { return &rhs_; }
	Vector* mutable_y() { return &y_; }
	Vector* mutable_z() { return &z_; }

	ContextImpl* context() { return &context_; }

	private:
	ContextImpl context_;

	std::unique_ptr<BlockSparseMatrix> matrix_;
	Vector b_;
	std::unique_ptr<BlockRandomAccessDenseMatrix> lhs_;
	Vector rhs_;
	Vector diagonal_;
	Vector z_;
	Vector y_;
	std::mt19937 prng_;
	std::normal_distribution<> standard_normal_;
	};

	static void BM_SchurEliminatorEliminate(benchmark::State& state) {
	const int num_e_blocks = state.range(0);
	BenchmarkData data(num_e_blocks);

	LinearSolver::Options linear_solver_options;
	linear_solver_options.e_block_size = kEBlockSize;
	linear_solver_options.row_block_size = kRowBlockSize;
	linear_solver_options.f_block_size = kFBlockSize;
	linear_solver_options.context = data.context();
	std::unique_ptr<SchurEliminatorBase> eliminator(
	SchurEliminatorBase::Create(linear_solver_options));

	eliminator->Init(num_e_blocks, true, data.matrix().block_structure());
	for (auto _ : state) {
	eliminator->Eliminate(BlockSparseMatrixData(data.matrix()),
	data.b().data(),
	data.diagonal().data(),
	data.mutable_lhs(),
	data.mutable_rhs()->data());
	}
	}

	static void BM_SchurEliminatorBackSubstitute(benchmark::State& state) {
	const int num_e_blocks = state.range(0);
	BenchmarkData data(num_e_blocks);

	LinearSolver::Options linear_solver_options;
	linear_solver_options.e_block_size = kEBlockSize;
	linear_solver_options.row_block_size = kRowBlockSize;
	linear_solver_options.f_block_size = kFBlockSize;
	linear_solver_options.context = data.context();
	std::unique_ptr<SchurEliminatorBase> eliminator(
	SchurEliminatorBase::Create(linear_solver_options));

	eliminator->Init(num_e_blocks, true, data.matrix().block_structure());
	eliminator->Eliminate(BlockSparseMatrixData(data.matrix()),
	data.b().data(),
	data.diagonal().data(),
	data.mutable_lhs(),
	data.mutable_rhs()->data());
	for (auto _ : state) {
	eliminator->BackSubstitute(BlockSparseMatrixData(data.matrix()),
	data.b().data(),
	data.diagonal().data(),
	data.mutable_z()->data(),
	data.mutable_y()->data());
	}
	}

	static void BM_SchurEliminatorForOneFBlockEliminate(benchmark::State& state) {
	const int num_e_blocks = state.range(0);
	BenchmarkData data(num_e_blocks);
	SchurEliminatorForOneFBlock<2, 3, 6> eliminator;
	eliminator.Init(num_e_blocks, true, data.matrix().block_structure());
	for (auto _ : state) {
	eliminator.Eliminate(BlockSparseMatrixData(data.matrix()),
	data.b().data(),
	data.diagonal().data(),
	data.mutable_lhs(),
	data.mutable_rhs()->data());
	}
	}

	static void BM_SchurEliminatorForOneFBlockBackSubstitute(
	benchmark::State& state) {
	const int num_e_blocks = state.range(0);
	BenchmarkData data(num_e_blocks);
	SchurEliminatorForOneFBlock<2, 3, 6> eliminator;
	eliminator.Init(num_e_blocks, true, data.matrix().block_structure());
	eliminator.Eliminate(BlockSparseMatrixData(data.matrix()),
	data.b().data(),
	data.diagonal().data(),
	data.mutable_lhs(),
	data.mutable_rhs()->data());
	for (auto _ : state) {
	eliminator.BackSubstitute(BlockSparseMatrixData(data.matrix()),
	data.b().data(),
	data.diagonal().data(),
	data.mutable_z()->data(),
	data.mutable_y()->data());
	}
	}

	BENCHMARK(BM_SchurEliminatorEliminate)->Range(10, 10000);
	BENCHMARK(BM_SchurEliminatorForOneFBlockEliminate)->Range(10, 10000);
	BENCHMARK(BM_SchurEliminatorBackSubstitute)->Range(10, 10000);
	BENCHMARK(BM_SchurEliminatorForOneFBlockBackSubstitute)->Range(10, 10000);

	} // namespace ceres::internal

	BENCHMARK_MAIN();