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


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

            
            // Copyright 2023 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: wjr@google.com (William Rucklidge),

            
            //          keir@google.com (Keir Mierle),

            
            //          dgossow@google.com (David Gossow)


            #include "ceres/gradient_checker.h"


            #include <algorithm>

            
            #include <cmath>

            
            #include <cstdint>

            
            #include <numeric>

            
            #include <string>

            
            #include <vector>


            #include "absl/log/check.h"

            
            #include "absl/log/log.h"

            
            #include "absl/strings/str_cat.h"

            
            #include "absl/strings/str_format.h"

            
            #include "ceres/is_close.h"

            
            #include "ceres/types.h"


            namespace ceres {


            using internal::IsClose;


            namespace {

            
            // Evaluate the cost function and transform the returned Jacobians to

            
            // the tangent space of the respective local parameterizations.

            
            bool EvaluateCostFunction(const CostFunction* function,

            
                                      double const* const* parameters,

            
                                      const std::vector<const Manifold*>& manifolds,

            
                                      Vector* residuals,

            
                                      std::vector<Matrix>* jacobians,

            
                                      std::vector<Matrix>* local_jacobians) {

            
              CHECK(residuals != nullptr);

            
              CHECK(jacobians != nullptr);

            
              CHECK(local_jacobians != nullptr);


              const std::vector<int32_t>& block_sizes = function->parameter_block_sizes();

            
              const int num_parameter_blocks = block_sizes.size();


              // Allocate Jacobian matrices in tangent space.

            
              local_jacobians->resize(num_parameter_blocks);

            
              std::vector<double*> local_jacobian_data(num_parameter_blocks);

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

            
                int block_size = block_sizes.at(i);

            
                if (manifolds.at(i) != nullptr) {

            
                  block_size = manifolds.at(i)->TangentSize();

            
                }

            
                local_jacobians->at(i).resize(function->num_residuals(), block_size);

            
                local_jacobians->at(i).setZero();

            
                local_jacobian_data.at(i) = local_jacobians->at(i).data();

            
              }


              // Allocate Jacobian matrices in ambient space.

            
              jacobians->resize(num_parameter_blocks);

            
              std::vector<double*> jacobian_data(num_parameter_blocks);

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

            
                jacobians->at(i).resize(function->num_residuals(), block_sizes.at(i));

            
                jacobians->at(i).setZero();

            
                jacobian_data.at(i) = jacobians->at(i).data();

            
              }


              // Compute residuals & jacobians.

            
              CHECK_NE(0, function->num_residuals());

            
              residuals->resize(function->num_residuals());

            
              residuals->setZero();

            
              if (!function->Evaluate(

            
                      parameters, residuals->data(), jacobian_data.data())) {

            
                return false;

            
              }


              // Convert Jacobians from ambient to local space.

            
              for (size_t i = 0; i < local_jacobians->size(); ++i) {

            
                if (manifolds.at(i) == nullptr) {

            
                  local_jacobians->at(i) = jacobians->at(i);

            
                } else {

            
                  int ambient_size = manifolds.at(i)->AmbientSize();

            
                  int tangent_size = manifolds.at(i)->TangentSize();

            
                  CHECK_EQ(jacobians->at(i).cols(), ambient_size);

            
                  Matrix ambient_J_tangent(ambient_size, tangent_size);

            
                  manifolds.at(i)->PlusJacobian(parameters[i], ambient_J_tangent.data());

            
                  local_jacobians->at(i).noalias() = jacobians->at(i) * ambient_J_tangent;

            
                }

            
              }

            
              return true;

            
            }

            
            }  // namespace


            GradientChecker::GradientChecker(const CostFunction* function,

            
                                             const std::vector<const Manifold*>* manifolds,

            
                                             const NumericDiffOptions& options)

            
                : function_(function) {

            
              CHECK(function != nullptr);

            
              if (manifolds != nullptr) {

            
                manifolds_ = *manifolds;

            
              } else {

            
                manifolds_.resize(function->parameter_block_sizes().size(), nullptr);

            
              }


              auto finite_diff_cost_function =

            
                  std::make_unique<DynamicNumericDiffCostFunction<CostFunction, RIDDERS>>(

            
                      function, DO_NOT_TAKE_OWNERSHIP, options);

            
              const std::vector<int32_t>& parameter_block_sizes =

            
                  function->parameter_block_sizes();

            
              const int num_parameter_blocks = parameter_block_sizes.size();

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

            
                finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]);

            
              }

            
              finite_diff_cost_function->SetNumResiduals(function->num_residuals());


              finite_diff_cost_function_ = std::move(finite_diff_cost_function);

            
            }


            bool GradientChecker::Probe(double const* const* parameters,

            
                                        double relative_precision,

            
                                        ProbeResults* results_param) const {

            
              int num_residuals = function_->num_residuals();


              // Make sure that we have a place to store results, no matter if the user has

            
              // provided an output argument.

            
              ProbeResults* results;

            
              ProbeResults results_local;

            
              if (results_param != nullptr) {

            
                results = results_param;

            
                results->residuals.resize(0);

            
                results->jacobians.clear();

            
                results->numeric_jacobians.clear();

            
                results->local_jacobians.clear();

            
                results->local_numeric_jacobians.clear();

            
                results->error_log.clear();

            
              } else {

            
                results = &results_local;

            
              }

            
              results->maximum_relative_error = 0.0;

            
              results->return_value = true;


              // Evaluate the derivative using the user supplied code.

            
              std::vector<Matrix>& jacobians = results->jacobians;

            
              std::vector<Matrix>& local_jacobians = results->local_jacobians;

            
              if (!EvaluateCostFunction(function_,

            
                                        parameters,

            
                                        manifolds_,

            
                                        &results->residuals,

            
                                        &jacobians,

            
                                        &local_jacobians)) {

            
                results->error_log = "Function evaluation with Jacobians failed.";

            
                results->return_value = false;

            
              }


              // Evaluate the derivative using numeric derivatives.

            
              std::vector<Matrix>& numeric_jacobians = results->numeric_jacobians;

            
              std::vector<Matrix>& local_numeric_jacobians =

            
                  results->local_numeric_jacobians;

            
              Vector finite_diff_residuals;

            
              if (!EvaluateCostFunction(finite_diff_cost_function_.get(),

            
                                        parameters,

            
                                        manifolds_,

            
                                        &finite_diff_residuals,

            
                                        &numeric_jacobians,

            
                                        &local_numeric_jacobians)) {

            
                absl::StrAppend(&results->error_log,

            
                                "\nFunction evaluation with numerical "

            
                                "differentiation failed.");

            
                results->return_value = false;

            
              }


              if (!results->return_value) {

            
                return false;

            
              }


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

            
                if (!IsClose(results->residuals[i],

            
                             finite_diff_residuals[i],

            
                             relative_precision,

            
                             nullptr,

            
                             nullptr)) {

            
                  results->error_log =

            
                      "Function evaluation with and without Jacobians "

            
                      "resulted in different residuals.";

            
                  LOG(INFO) << results->residuals.transpose();

            
                  LOG(INFO) << finite_diff_residuals.transpose();

            
                  return false;

            
                }

            
              }


              // See if any elements have relative error larger than the threshold.

            
              int num_bad_jacobian_components = 0;

            
              double& worst_relative_error = results->maximum_relative_error;

            
              worst_relative_error = 0;


              // Accumulate the error message for all the jacobians, since it won't get

            
              // output if there are no bad jacobian components.

            
              std::string error_log;

            
              for (int k = 0; k < function_->parameter_block_sizes().size(); k++) {

            
                absl::StrAppendFormat(&error_log,

            
                                      "========== "

            
                                      "Jacobian for block %d: (%ld by %ld)) "

            
                                      "==========\n",

            
                                      k,

            
                                      static_cast<long>(local_jacobians[k].rows()),

            
                                      static_cast<long>(local_jacobians[k].cols()));

            
                // The funny spacing creates appropriately aligned column headers.

            
                absl::StrAppend(

            
                    &error_log,

            
                    " block  row  col        user dx/dy    num diff dx/dy         "

            
                    "abs error    relative error         parameter          residual\n");


                for (int i = 0; i < local_jacobians[k].rows(); i++) {

            
                  for (int j = 0; j < local_jacobians[k].cols(); j++) {

            
                    double term_jacobian = local_jacobians[k](i, j);

            
                    double finite_jacobian = local_numeric_jacobians[k](i, j);

            
                    double relative_error, absolute_error;

            
                    bool bad_jacobian_entry = !IsClose(term_jacobian,

            
                                                       finite_jacobian,

            
                                                       relative_precision,

            
                                                       &relative_error,

            
                                                       &absolute_error);

            
                    worst_relative_error = std::max(worst_relative_error, relative_error);


                    absl::StrAppendFormat(&error_log,

            
                                          "%6d %4d %4d %17g %17g %17g %17g %17g %17g",

            
                                          k,

            
                                          i,

            
                                          j,

            
                                          term_jacobian,

            
                                          finite_jacobian,

            
                                          absolute_error,

            
                                          relative_error,

            
                                          parameters[k][j],

            
                                          results->residuals[i]);


                    if (bad_jacobian_entry) {

            
                      num_bad_jacobian_components++;

            
                      absl::StrAppendFormat(

            
                          &error_log,

            
                          " ------ (%d,%d,%d) Relative error worse than %g",

            
                          k,

            
                          i,

            
                          j,

            
                          relative_precision);

            
                    }

            
                    absl::StrAppend(&error_log, "\n");

            
                  }

            
                }

            
              }


              // Since there were some bad errors, dump comprehensive debug info.

            
              if (num_bad_jacobian_components) {

            
                std::string header = absl::StrFormat(

            
                    "\nDetected %d bad Jacobian component(s). "

            
                    "Worst relative error was %g.\n",

            
                    num_bad_jacobian_components,

            
                    worst_relative_error);


                results->error_log = absl::StrCat(header, "\n", error_log);

            
                return false;

            
              }

            
              return true;

            
            }


            }  // namespace ceres
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2023 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: wjr@google.com (William Rucklidge),
	// keir@google.com (Keir Mierle),
	// dgossow@google.com (David Gossow)

	#include "ceres/gradient_checker.h"

	#include <algorithm>
	#include <cmath>
	#include <cstdint>
	#include <numeric>
	#include <string>
	#include <vector>

	#include "absl/log/check.h"
	#include "absl/log/log.h"
	#include "absl/strings/str_cat.h"
	#include "absl/strings/str_format.h"
	#include "ceres/is_close.h"
	#include "ceres/types.h"

	namespace ceres {

	using internal::IsClose;

	namespace {
	// Evaluate the cost function and transform the returned Jacobians to
	// the tangent space of the respective local parameterizations.
	bool EvaluateCostFunction(const CostFunction* function,
	double const* const* parameters,
	const std::vector<const Manifold*>& manifolds,
	Vector* residuals,
	std::vector<Matrix>* jacobians,
	std::vector<Matrix>* local_jacobians) {
	CHECK(residuals != nullptr);
	CHECK(jacobians != nullptr);
	CHECK(local_jacobians != nullptr);

	const std::vector<int32_t>& block_sizes = function->parameter_block_sizes();
	const int num_parameter_blocks = block_sizes.size();

	// Allocate Jacobian matrices in tangent space.
	local_jacobians->resize(num_parameter_blocks);
	std::vector<double*> local_jacobian_data(num_parameter_blocks);
	for (int i = 0; i < num_parameter_blocks; ++i) {
	int block_size = block_sizes.at(i);
	if (manifolds.at(i) != nullptr) {
	block_size = manifolds.at(i)->TangentSize();
	}
	local_jacobians->at(i).resize(function->num_residuals(), block_size);
	local_jacobians->at(i).setZero();
	local_jacobian_data.at(i) = local_jacobians->at(i).data();
	}

	// Allocate Jacobian matrices in ambient space.
	jacobians->resize(num_parameter_blocks);
	std::vector<double*> jacobian_data(num_parameter_blocks);
	for (int i = 0; i < num_parameter_blocks; ++i) {
	jacobians->at(i).resize(function->num_residuals(), block_sizes.at(i));
	jacobians->at(i).setZero();
	jacobian_data.at(i) = jacobians->at(i).data();
	}

	// Compute residuals & jacobians.
	CHECK_NE(0, function->num_residuals());
	residuals->resize(function->num_residuals());
	residuals->setZero();
	if (!function->Evaluate(
	parameters, residuals->data(), jacobian_data.data())) {
	return false;
	}

	// Convert Jacobians from ambient to local space.
	for (size_t i = 0; i < local_jacobians->size(); ++i) {
	if (manifolds.at(i) == nullptr) {
	local_jacobians->at(i) = jacobians->at(i);
	} else {
	int ambient_size = manifolds.at(i)->AmbientSize();
	int tangent_size = manifolds.at(i)->TangentSize();
	CHECK_EQ(jacobians->at(i).cols(), ambient_size);
	Matrix ambient_J_tangent(ambient_size, tangent_size);
	manifolds.at(i)->PlusJacobian(parameters[i], ambient_J_tangent.data());
	local_jacobians->at(i).noalias() = jacobians->at(i) * ambient_J_tangent;
	}
	}
	return true;
	}
	} // namespace

	GradientChecker::GradientChecker(const CostFunction* function,
	const std::vector<const Manifold> manifolds,
	const NumericDiffOptions& options)
	: function_(function) {
	CHECK(function != nullptr);
	if (manifolds != nullptr) {
	manifolds_ = *manifolds;
	} else {
	manifolds_.resize(function->parameter_block_sizes().size(), nullptr);
	}

	auto finite_diff_cost_function =
	std::make_unique<DynamicNumericDiffCostFunction<CostFunction, RIDDERS>>(
	function, DO_NOT_TAKE_OWNERSHIP, options);
	const std::vector<int32_t>& parameter_block_sizes =
	function->parameter_block_sizes();
	const int num_parameter_blocks = parameter_block_sizes.size();
	for (int i = 0; i < num_parameter_blocks; ++i) {
	finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]);
	}
	finite_diff_cost_function->SetNumResiduals(function->num_residuals());

	finite_diff_cost_function_ = std::move(finite_diff_cost_function);
	}

	bool GradientChecker::Probe(double const* const* parameters,
	double relative_precision,
	ProbeResults* results_param) const {
	int num_residuals = function_->num_residuals();

	// Make sure that we have a place to store results, no matter if the user has
	// provided an output argument.
	ProbeResults* results;
	ProbeResults results_local;
	if (results_param != nullptr) {
	results = results_param;
	results->residuals.resize(0);
	results->jacobians.clear();
	results->numeric_jacobians.clear();
	results->local_jacobians.clear();
	results->local_numeric_jacobians.clear();
	results->error_log.clear();
	} else {
	results = &results_local;
	}
	results->maximum_relative_error = 0.0;
	results->return_value = true;

	// Evaluate the derivative using the user supplied code.
	std::vector<Matrix>& jacobians = results->jacobians;
	std::vector<Matrix>& local_jacobians = results->local_jacobians;
	if (!EvaluateCostFunction(function_,
	parameters,
	manifolds_,
	&results->residuals,
	&jacobians,
	&local_jacobians)) {
	results->error_log = "Function evaluation with Jacobians failed.";
	results->return_value = false;
	}

	// Evaluate the derivative using numeric derivatives.
	std::vector<Matrix>& numeric_jacobians = results->numeric_jacobians;
	std::vector<Matrix>& local_numeric_jacobians =
	results->local_numeric_jacobians;
	Vector finite_diff_residuals;
	if (!EvaluateCostFunction(finite_diff_cost_function_.get(),
	parameters,
	manifolds_,
	&finite_diff_residuals,
	&numeric_jacobians,
	&local_numeric_jacobians)) {
	absl::StrAppend(&results->error_log,
	"\nFunction evaluation with numerical "
	"differentiation failed.");
	results->return_value = false;
	}

	if (!results->return_value) {
	return false;
	}

	for (int i = 0; i < num_residuals; ++i) {
	if (!IsClose(results->residuals[i],
	finite_diff_residuals[i],
	relative_precision,
	nullptr,
	nullptr)) {
	results->error_log =
	"Function evaluation with and without Jacobians "
	"resulted in different residuals.";
	LOG(INFO) << results->residuals.transpose();
	LOG(INFO) << finite_diff_residuals.transpose();
	return false;
	}
	}

	// See if any elements have relative error larger than the threshold.
	int num_bad_jacobian_components = 0;
	double& worst_relative_error = results->maximum_relative_error;
	worst_relative_error = 0;

	// Accumulate the error message for all the jacobians, since it won't get
	// output if there are no bad jacobian components.
	std::string error_log;
	for (int k = 0; k < function_->parameter_block_sizes().size(); k++) {
	absl::StrAppendFormat(&error_log,
	"========== "
	"Jacobian for block %d: (%ld by %ld)) "
	"==========\n",
	k,
	static_cast<long>(local_jacobians[k].rows()),
	static_cast<long>(local_jacobians[k].cols()));
	// The funny spacing creates appropriately aligned column headers.
	absl::StrAppend(
	&error_log,
	" block row col user dx/dy num diff dx/dy "
	"abs error relative error parameter residual\n");

	for (int i = 0; i < local_jacobians[k].rows(); i++) {
	for (int j = 0; j < local_jacobians[k].cols(); j++) {
	double term_jacobian = local_jacobians[k](i, j);
	double finite_jacobian = local_numeric_jacobians[k](i, j);
	double relative_error, absolute_error;
	bool bad_jacobian_entry = !IsClose(term_jacobian,
	finite_jacobian,
	relative_precision,
	&relative_error,
	&absolute_error);
	worst_relative_error = std::max(worst_relative_error, relative_error);

	absl::StrAppendFormat(&error_log,
	"%6d %4d %4d %17g %17g %17g %17g %17g %17g",
	k,
	i,
	j,
	term_jacobian,
	finite_jacobian,
	absolute_error,
	relative_error,
	parameters[k][j],
	results->residuals[i]);

	if (bad_jacobian_entry) {
	num_bad_jacobian_components++;
	absl::StrAppendFormat(
	&error_log,
	" ------ (%d,%d,%d) Relative error worse than %g",
	k,
	i,
	j,
	relative_precision);
	}
	absl::StrAppend(&error_log, "\n");
	}
	}
	}

	// Since there were some bad errors, dump comprehensive debug info.
	if (num_bad_jacobian_components) {
	std::string header = absl::StrFormat(
	"\nDetected %d bad Jacobian component(s). "
	"Worst relative error was %g.\n",
	num_bad_jacobian_components,
	worst_relative_error);

	results->error_log = absl::StrCat(header, "\n", error_log);
	return false;
	}
	return true;
	}

	} // namespace ceres