Vehicle-cpp/zjw/ceres-solver-master/internal/ceres/gradient_checking_cost_function.cc

// 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: keir@google.com (Keir Mierle),
//          dgossow@google.com (David Gossow)

#include "ceres/gradient_checking_cost_function.h"

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

#include "ceres/dynamic_numeric_diff_cost_function.h"
#include "ceres/gradient_checker.h"
#include "ceres/internal/eigen.h"
#include "ceres/parameter_block.h"
#include "ceres/problem.h"
#include "ceres/problem_impl.h"
#include "ceres/program.h"
#include "ceres/residual_block.h"
#include "ceres/stringprintf.h"
#include "ceres/types.h"
#include "glog/logging.h"

namespace ceres::internal {

namespace {

class GradientCheckingCostFunction final : public CostFunction {
 public:
  GradientCheckingCostFunction(const CostFunction* function,
                               const std::vector<const Manifold*>* manifolds,
                               const NumericDiffOptions& options,
                               double relative_precision,
                               std::string extra_info,
                               GradientCheckingIterationCallback* callback)
      : function_(function),
        gradient_checker_(function, manifolds, options),
        relative_precision_(relative_precision),
        extra_info_(std::move(extra_info)),
        callback_(callback) {
    CHECK(callback_ != nullptr);
    const std::vector<int32_t>& parameter_block_sizes =
        function->parameter_block_sizes();
    *mutable_parameter_block_sizes() = parameter_block_sizes;
    set_num_residuals(function->num_residuals());
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const final {
    if (!jacobians) {
      // Nothing to check in this case; just forward.
      return function_->Evaluate(parameters, residuals, nullptr);
    }

    GradientChecker::ProbeResults results;
    bool okay =
        gradient_checker_.Probe(parameters, relative_precision_, &results);

    // If the cost function returned false, there's nothing we can say about
    // the gradients.
    if (results.return_value == false) {
      return false;
    }

    // Copy the residuals.
    const int num_residuals = function_->num_residuals();
    MatrixRef(residuals, num_residuals, 1) = results.residuals;

    // Copy the original jacobian blocks into the jacobians array.
    const std::vector<int32_t>& block_sizes =
        function_->parameter_block_sizes();
    for (int k = 0; k < block_sizes.size(); k++) {
      if (jacobians[k] != nullptr) {
        MatrixRef(jacobians[k],
                  results.jacobians[k].rows(),
                  results.jacobians[k].cols()) = results.jacobians[k];
      }
    }

    if (!okay) {
      std::string error_log =
          "Gradient Error detected!\nExtra info for this residual: " +
          extra_info_ + "\n" + results.error_log;
      callback_->SetGradientErrorDetected(error_log);
    }
    return true;
  }

 private:
  const CostFunction* function_;
  GradientChecker gradient_checker_;
  double relative_precision_;
  std::string extra_info_;
  GradientCheckingIterationCallback* callback_;
};

}  // namespace

GradientCheckingIterationCallback::GradientCheckingIterationCallback()
    : gradient_error_detected_(false) {}

CallbackReturnType GradientCheckingIterationCallback::operator()(
    const IterationSummary& /*summary*/) {
  if (gradient_error_detected_) {
    LOG(ERROR) << "Gradient error detected. Terminating solver.";
    return SOLVER_ABORT;
  }
  return SOLVER_CONTINUE;
}

void GradientCheckingIterationCallback::SetGradientErrorDetected(
    std::string& error_log) {
  std::lock_guard<std::mutex> l(mutex_);
  gradient_error_detected_ = true;
  error_log_ += "\n" + error_log;
}

std::unique_ptr<CostFunction> CreateGradientCheckingCostFunction(
    const CostFunction* cost_function,
    const std::vector<const Manifold*>* manifolds,
    double relative_step_size,
    double relative_precision,
    const std::string& extra_info,
    GradientCheckingIterationCallback* callback) {
  NumericDiffOptions numeric_diff_options;
  numeric_diff_options.relative_step_size = relative_step_size;

  return std::make_unique<GradientCheckingCostFunction>(cost_function,
                                                        manifolds,
                                                        numeric_diff_options,
                                                        relative_precision,
                                                        extra_info,
                                                        callback);
}

std::unique_ptr<ProblemImpl> CreateGradientCheckingProblemImpl(
    ProblemImpl* problem_impl,
    double relative_step_size,
    double relative_precision,
    GradientCheckingIterationCallback* callback) {
  CHECK(callback != nullptr);
  // We create new CostFunctions by wrapping the original CostFunction in a
  // gradient checking CostFunction. So its okay for the ProblemImpl to take
  // ownership of it and destroy it. The LossFunctions and Manifolds are reused
  // and since they are owned by problem_impl, gradient_checking_problem_impl
  // should not take ownership of it.
  Problem::Options gradient_checking_problem_options;
  gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP;
  gradient_checking_problem_options.loss_function_ownership =
      DO_NOT_TAKE_OWNERSHIP;
  gradient_checking_problem_options.manifold_ownership = DO_NOT_TAKE_OWNERSHIP;
  gradient_checking_problem_options.context = problem_impl->context();

  NumericDiffOptions numeric_diff_options;
  numeric_diff_options.relative_step_size = relative_step_size;

  auto gradient_checking_problem_impl =
      std::make_unique<ProblemImpl>(gradient_checking_problem_options);

  Program* program = problem_impl->mutable_program();

  // For every ParameterBlock in problem_impl, create a new parameter block with
  // the same manifold and constancy.
  const std::vector<ParameterBlock*>& parameter_blocks =
      program->parameter_blocks();
  for (auto* parameter_block : parameter_blocks) {
    gradient_checking_problem_impl->AddParameterBlock(
        parameter_block->mutable_user_state(),
        parameter_block->Size(),
        parameter_block->mutable_manifold());

    if (parameter_block->IsConstant()) {
      gradient_checking_problem_impl->SetParameterBlockConstant(
          parameter_block->mutable_user_state());
    }

    for (int i = 0; i < parameter_block->Size(); ++i) {
      gradient_checking_problem_impl->SetParameterUpperBound(
          parameter_block->mutable_user_state(),
          i,
          parameter_block->UpperBound(i));
      gradient_checking_problem_impl->SetParameterLowerBound(
          parameter_block->mutable_user_state(),
          i,
          parameter_block->LowerBound(i));
    }
  }

  // For every ResidualBlock in problem_impl, create a new
  // ResidualBlock by wrapping its CostFunction inside a
  // GradientCheckingCostFunction.
  const std::vector<ResidualBlock*>& residual_blocks =
      program->residual_blocks();
  for (int i = 0; i < residual_blocks.size(); ++i) {
    ResidualBlock* residual_block = residual_blocks[i];

    // Build a human readable string which identifies the
    // ResidualBlock. This is used by the GradientCheckingCostFunction
    // when logging debugging information.
    std::string extra_info =
        StringPrintf("Residual block id %d; depends on parameters [", i);
    std::vector<double*> parameter_blocks;
    std::vector<const Manifold*> manifolds;
    parameter_blocks.reserve(residual_block->NumParameterBlocks());
    manifolds.reserve(residual_block->NumParameterBlocks());
    for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
      ParameterBlock* parameter_block = residual_block->parameter_blocks()[j];
      parameter_blocks.push_back(parameter_block->mutable_user_state());
      StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state());
      extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]";
      manifolds.push_back(
          problem_impl->GetManifold(parameter_block->mutable_user_state()));
    }

    // Wrap the original CostFunction in a GradientCheckingCostFunction.
    CostFunction* gradient_checking_cost_function =
        new GradientCheckingCostFunction(residual_block->cost_function(),
                                         &manifolds,
                                         numeric_diff_options,
                                         relative_precision,
                                         extra_info,
                                         callback);

    // The const_cast is necessary because
    // ProblemImpl::AddResidualBlock can potentially take ownership of
    // the LossFunction, but in this case we are guaranteed that this
    // will not be the case, so this const_cast is harmless.
    gradient_checking_problem_impl->AddResidualBlock(
        gradient_checking_cost_function,
        const_cast<LossFunction*>(residual_block->loss_function()),
        parameter_blocks.data(),
        static_cast<int>(parameter_blocks.size()));
  }

  // Normally, when a problem is given to the solver, we guarantee
  // that the state pointers for each parameter block point to the
  // user provided data. Since we are creating this new problem from a
  // problem given to us at an arbitrary stage of the solve, we cannot
  // depend on this being the case, so we explicitly call
  // SetParameterBlockStatePtrsToUserStatePtrs to ensure that this is
  // the case.
  gradient_checking_problem_impl->mutable_program()
      ->SetParameterBlockStatePtrsToUserStatePtrs();

  return gradient_checking_problem_impl;
}

}  // namespace ceres::internal