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

// Copyright 2015 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 <numeric>

#include <string>

#include <vector>

#include "ceres/gradient_checker.h"

#include "ceres/internal/eigen.h"

#include "ceres/internal/scoped_ptr.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/dynamic_numeric_diff_cost_function.h"

#include "ceres/stringprintf.h"

#include "ceres/types.h"

#include "glog/logging.h"

namespace ceres {

namespace internal {

using std::abs;

using std::max;

using std::string;

using std::vector;

namespace {

class GradientCheckingCostFunction : public CostFunction {

 public:

  GradientCheckingCostFunction(

      const CostFunction* function,

      const std::vector<const LocalParameterization*>* local_parameterizations,

      const NumericDiffOptions& options,

      double relative_precision,

      const string& extra_info,

      GradientCheckingIterationCallback* callback)

      : function_(function),

        gradient_checker_(function, local_parameterizations, options),

        relative_precision_(relative_precision),

        extra_info_(extra_info),

        callback_(callback) {

    CHECK_NOTNULL(callback_);

    const vector<int32>& parameter_block_sizes =

        function->parameter_block_sizes();

    *mutable_parameter_block_sizes() = parameter_block_sizes;

    set_num_residuals(function->num_residuals());

  }

  virtual ~GradientCheckingCostFunction() { }

  virtual bool Evaluate(double const* const* parameters,

                        double* residuals,

                        double** jacobians) const {

    if (!jacobians) {

      // Nothing to check in this case; just forward.

      return function_->Evaluate(parameters, residuals, NULL);

    }

    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 vector<int32>& block_sizes = function_->parameter_block_sizes();

    for (int k = 0; k < block_sizes.size(); k++) {

      if (jacobians[k] != NULL) {

        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_;

  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) {

  mutex_.Lock();

  gradient_error_detected_ = true;

  error_log_ += "\n" + error_log;

  mutex_.Unlock();

}

CostFunction* CreateGradientCheckingCostFunction(

    const CostFunction* cost_function,

    const std::vector<const LocalParameterization*>* local_parameterizations,

    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 new GradientCheckingCostFunction(cost_function,

                                          local_parameterizations,

                                          numeric_diff_options,

                                          relative_precision, extra_info,

                                          callback);

}

ProblemImpl* CreateGradientCheckingProblemImpl(

    ProblemImpl* problem_impl,

    double relative_step_size,

    double relative_precision,

    GradientCheckingIterationCallback* callback) {

  CHECK_NOTNULL(callback);

  // 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 LocalParameterizations 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.local_parameterization_ownership =

      DO_NOT_TAKE_OWNERSHIP;

  NumericDiffOptions numeric_diff_options;

  numeric_diff_options.relative_step_size = relative_step_size;

  ProblemImpl* gradient_checking_problem_impl = new 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 local parameterization and constancy.

  const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();

  for (int i = 0; i < parameter_blocks.size(); ++i) {

    ParameterBlock* parameter_block = parameter_blocks[i];

    gradient_checking_problem_impl->AddParameterBlock(

        parameter_block->mutable_user_state(),

        parameter_block->Size(),

        parameter_block->mutable_local_parameterization());

    if (parameter_block->IsConstant()) {

      gradient_checking_problem_impl->SetParameterBlockConstant(

          parameter_block->mutable_user_state());

    }

  }

  // For every ResidualBlock in problem_impl, create a new

  // ResidualBlock by wrapping its CostFunction inside a

  // GradientCheckingCostFunction.

  const 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.

    string extra_info = StringPrintf(

        "Residual block id %d; depends on parameters [", i);

    vector<double*> parameter_blocks;

    vector<const LocalParameterization*> local_parameterizations;

    parameter_blocks.reserve(residual_block->NumParameterBlocks());

    local_parameterizations.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) ? ", " : "]";

      local_parameterizations.push_back(problem_impl->GetParameterization(

          parameter_block->mutable_user_state()));

    }

    // Wrap the original CostFunction in a GradientCheckingCostFunction.

    CostFunction* gradient_checking_cost_function =

        new GradientCheckingCostFunction(residual_block->cost_function(),

                                         &local_parameterizations,

                                         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);

  }

  // 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 internal

}  // namespace ceres