// 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.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
//         keir@google.com (Keir Mierle)
//
// The Problem object is used to build and hold least squares problems.

#ifndef CERES_PUBLIC_PROBLEM_H_
#define CERES_PUBLIC_PROBLEM_H_

#include <array>
#include <cstddef>
#include <map>
#include <memory>
#include <set>
#include <vector>

#include "ceres/context.h"
#include "ceres/internal/disable_warnings.h"
#include "ceres/internal/port.h"
#include "ceres/types.h"
#include "glog/logging.h"

namespace ceres {

class CostFunction;
class LossFunction;
class LocalParameterization;
class Solver;
struct CRSMatrix;

namespace internal {
class Preprocessor;
class ProblemImpl;
class ParameterBlock;
class ResidualBlock;
}  // namespace internal

// A ResidualBlockId is an opaque handle clients can use to remove residual
// blocks from a Problem after adding them.
typedef internal::ResidualBlock* ResidualBlockId;

// A class to represent non-linear least squares problems. Such
// problems have a cost function that is a sum of error terms (known
// as "residuals"), where each residual is a function of some subset
// of the parameters. The cost function takes the form
//
//    N    1
//   SUM  --- loss( || r_i1, r_i2,..., r_ik ||^2  ),
//   i=1   2
//
// where
//
//   r_ij     is residual number i, component j; the residual is a
//            function of some subset of the parameters x1...xk. For
//            example, in a structure from motion problem a residual
//            might be the difference between a measured point in an
//            image and the reprojected position for the matching
//            camera, point pair. The residual would have two
//            components, error in x and error in y.
//
//   loss(y)  is the loss function; for example, squared error or
//            Huber L1 loss. If loss(y) = y, then the cost function is
//            non-robustified least squares.
//
// This class is specifically designed to address the important subset
// of "sparse" least squares problems, where each component of the
// residual depends only on a small number number of parameters, even
// though the total number of residuals and parameters may be very
// large. This property affords tremendous gains in scale, allowing
// efficient solving of large problems that are otherwise
// inaccessible.
//
// The canonical example of a sparse least squares problem is
// "structure-from-motion" (SFM), where the parameters are points and
// cameras, and residuals are reprojection errors. Typically a single
// residual will depend only on 9 parameters (3 for the point, 6 for
// the camera).
//
// To create a least squares problem, use the AddResidualBlock() and
// AddParameterBlock() methods, documented below. Here is an example least
// squares problem containing 3 parameter blocks of sizes 3, 4 and 5
// respectively and two residual terms of size 2 and 6:
//
//   double x1[] = { 1.0, 2.0, 3.0 };
//   double x2[] = { 1.0, 2.0, 3.0, 5.0 };
//   double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 };
//
//   Problem problem;
//
//   problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1);
//   problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x3);
//
// Please see cost_function.h for details of the CostFunction object.
class CERES_EXPORT Problem {
 public:
  struct CERES_EXPORT Options {
    // These flags control whether the Problem object owns the cost
    // functions, loss functions, and parameterizations passed into
    // the Problem. If set to TAKE_OWNERSHIP, then the problem object
    // will delete the corresponding cost or loss functions on
    // destruction. The destructor is careful to delete the pointers
    // only once, since sharing cost/loss/parameterizations is
    // allowed.
    Ownership cost_function_ownership = TAKE_OWNERSHIP;
    Ownership loss_function_ownership = TAKE_OWNERSHIP;
    Ownership local_parameterization_ownership = TAKE_OWNERSHIP;

    // If true, trades memory for faster RemoveResidualBlock() and
    // RemoveParameterBlock() operations.
    //
    // By default, RemoveParameterBlock() and RemoveResidualBlock() take time
    // proportional to the size of the entire problem.  If you only ever remove
    // parameters or residuals from the problem occasionally, this might be
    // acceptable.  However, if you have memory to spare, enable this option to
    // make RemoveParameterBlock() take time proportional to the number of
    // residual blocks that depend on it, and RemoveResidualBlock() take (on
    // average) constant time.
    //
    // The increase in memory usage is twofold: an additional hash set per
    // parameter block containing all the residuals that depend on the parameter
    // block; and a hash set in the problem containing all residuals.
    bool enable_fast_removal = false;

    // By default, Ceres performs a variety of safety checks when constructing
    // the problem. There is a small but measurable performance penalty to
    // these checks, typically around 5% of construction time. If you are sure
    // your problem construction is correct, and 5% of the problem construction
    // time is truly an overhead you want to avoid, then you can set
    // disable_all_safety_checks to true.
    //
    // WARNING: Do not set this to true, unless you are absolutely sure of what
    // you are doing.
    bool disable_all_safety_checks = false;

    // A Ceres global context to use for solving this problem. This may help to
    // reduce computation time as Ceres can reuse expensive objects to create.
    // The context object can be nullptr, in which case Ceres may create one.
    //
    // Ceres does NOT take ownership of the pointer.
    Context* context = nullptr;
  };

  // The default constructor is equivalent to the
  // invocation Problem(Problem::Options()).
  Problem();
  explicit Problem(const Options& options);
  Problem(const Problem&) = delete;
  void operator=(const Problem&) = delete;

  ~Problem();

  // Add a residual block to the overall cost function. The cost
  // function carries with its information about the sizes of the
  // parameter blocks it expects. The function checks that these match
  // the sizes of the parameter blocks listed in parameter_blocks. The
  // program aborts if a mismatch is detected. loss_function can be
  // nullptr, in which case the cost of the term is just the squared norm
  // of the residuals.
  //
  // The user has the option of explicitly adding the parameter blocks
  // using AddParameterBlock. This causes additional correctness
  // checking; however, AddResidualBlock implicitly adds the parameter
  // blocks if they are not present, so calling AddParameterBlock
  // explicitly is not required.
  //
  // The Problem object by default takes ownership of the
  // cost_function and loss_function pointers. These objects remain
  // live for the life of the Problem object. If the user wishes to
  // keep control over the destruction of these objects, then they can
  // do this by setting the corresponding enums in the Options struct.
  //
  // Note: Even though the Problem takes ownership of cost_function
  // and loss_function, it does not preclude the user from re-using
  // them in another residual block. The destructor takes care to call
  // delete on each cost_function or loss_function pointer only once,
  // regardless of how many residual blocks refer to them.
  //
  // Example usage:
  //
  //   double x1[] = {1.0, 2.0, 3.0};
  //   double x2[] = {1.0, 2.0, 5.0, 6.0};
  //   double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0};
  //
  //   Problem problem;
  //
  //   problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1);
  //   problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1);
  //
  // Add a residual block by listing the parameter block pointers
  // directly instead of wapping them in a container.
  template <typename... Ts>
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0,
                                   Ts*... xs) {
    const std::array<double*, sizeof...(Ts) + 1> parameter_blocks{{x0, xs...}};
    return AddResidualBlock(cost_function, loss_function,
                            parameter_blocks.data(),
                            static_cast<int>(parameter_blocks.size()));
  }

  // Add a residual block by providing a vector of parameter blocks.
  ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      const std::vector<double*>& parameter_blocks);

  // Add a residual block by providing a pointer to the parameter block array
  // and the number of parameter blocks.
  ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      double* const* const parameter_blocks,
      int num_parameter_blocks);

  // Add a parameter block with appropriate size to the problem.
  // Repeated calls with the same arguments are ignored. Repeated
  // calls with the same double pointer but a different size results
  // in undefined behaviour.
  void AddParameterBlock(double* values, int size);

  // Add a parameter block with appropriate size and parameterization
  // to the problem. Repeated calls with the same arguments are
  // ignored. Repeated calls with the same double pointer but a
  // different size results in undefined behaviour.
  void AddParameterBlock(double* values,
                         int size,
                         LocalParameterization* local_parameterization);

  // Remove a parameter block from the problem. The parameterization of the
  // parameter block, if it exists, will persist until the deletion of the
  // problem (similar to cost/loss functions in residual block removal). Any
  // residual blocks that depend on the parameter are also removed, as
  // described above in RemoveResidualBlock().
  //
  // If Problem::Options::enable_fast_removal is true, then the
  // removal is fast (almost constant time). Otherwise, removing a parameter
  // block will incur a scan of the entire Problem object.
  //
  // WARNING: Removing a residual or parameter block will destroy the implicit
  // ordering, rendering the jacobian or residuals returned from the solver
  // uninterpretable. If you depend on the evaluated jacobian, do not use
  // remove! This may change in a future release.
  void RemoveParameterBlock(const double* values);

  // Remove a residual block from the problem. Any parameters that the residual
  // block depends on are not removed. The cost and loss functions for the
  // residual block will not get deleted immediately; won't happen until the
  // problem itself is deleted.
  //
  // WARNING: Removing a residual or parameter block will destroy the implicit
  // ordering, rendering the jacobian or residuals returned from the solver
  // uninterpretable. If you depend on the evaluated jacobian, do not use
  // remove! This may change in a future release.
  void RemoveResidualBlock(ResidualBlockId residual_block);

  // Hold the indicated parameter block constant during optimization.
  void SetParameterBlockConstant(const double* values);

  // Allow the indicated parameter block to vary during optimization.
  void SetParameterBlockVariable(double* values);

  // Returns true if a parameter block is set constant, and false otherwise.
  bool IsParameterBlockConstant(const double* values) const;

  // Set the local parameterization for one of the parameter blocks.
  // The local_parameterization is owned by the Problem by default. It
  // is acceptable to set the same parameterization for multiple
  // parameters; the destructor is careful to delete local
  // parameterizations only once. The local parameterization can only
  // be set once per parameter, and cannot be changed once set.
  void SetParameterization(double* values,
                           LocalParameterization* local_parameterization);

  // Get the local parameterization object associated with this
  // parameter block. If there is no parameterization object
  // associated then nullptr is returned.
  const LocalParameterization* GetParameterization(const double* values) const;

  // Set the lower/upper bound for the parameter at position "index".
  void SetParameterLowerBound(double* values, int index, double lower_bound);
  void SetParameterUpperBound(double* values, int index, double upper_bound);

  // Get the lower/upper bound for the parameter at position
  // "index". If the parameter is not bounded by the user, then its
  // lower bound is -std::numeric_limits<double>::max() and upper
  // bound is std::numeric_limits<double>::max().
  double GetParameterLowerBound(const double* values, int index) const;
  double GetParameterUpperBound(const double* values, int index) const;

  // Number of parameter blocks in the problem. Always equals
  // parameter_blocks().size() and parameter_block_sizes().size().
  int NumParameterBlocks() const;

  // The size of the parameter vector obtained by summing over the
  // sizes of all the parameter blocks.
  int NumParameters() const;

  // Number of residual blocks in the problem. Always equals
  // residual_blocks().size().
  int NumResidualBlocks() const;

  // The size of the residual vector obtained by summing over the
  // sizes of all of the residual blocks.
  int NumResiduals() const;

  // The size of the parameter block.
  int ParameterBlockSize(const double* values) const;

  // The size of local parameterization for the parameter block. If
  // there is no local parameterization associated with this parameter
  // block, then ParameterBlockLocalSize = ParameterBlockSize.
  int ParameterBlockLocalSize(const double* values) const;

  // Is the given parameter block present in this problem or not?
  bool HasParameterBlock(const double* values) const;

  // Fills the passed parameter_blocks vector with pointers to the
  // parameter blocks currently in the problem. After this call,
  // parameter_block.size() == NumParameterBlocks.
  void GetParameterBlocks(std::vector<double*>* parameter_blocks) const;

  // Fills the passed residual_blocks vector with pointers to the
  // residual blocks currently in the problem. After this call,
  // residual_blocks.size() == NumResidualBlocks.
  void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const;

  // Get all the parameter blocks that depend on the given residual block.
  void GetParameterBlocksForResidualBlock(
      const ResidualBlockId residual_block,
      std::vector<double*>* parameter_blocks) const;

  // Get the CostFunction for the given residual block.
  const CostFunction* GetCostFunctionForResidualBlock(
      const ResidualBlockId residual_block) const;

  // Get the LossFunction for the given residual block. Returns nullptr
  // if no loss function is associated with this residual block.
  const LossFunction* GetLossFunctionForResidualBlock(
      const ResidualBlockId residual_block) const;

  // Get all the residual blocks that depend on the given parameter block.
  //
  // If Problem::Options::enable_fast_removal is true, then
  // getting the residual blocks is fast and depends only on the number of
  // residual blocks. Otherwise, getting the residual blocks for a parameter
  // block will incur a scan of the entire Problem object.
  void GetResidualBlocksForParameterBlock(
      const double* values,
      std::vector<ResidualBlockId>* residual_blocks) const;

  // Options struct to control Problem::Evaluate.
  struct EvaluateOptions {
    // The set of parameter blocks for which evaluation should be
    // performed. This vector determines the order that parameter
    // blocks occur in the gradient vector and in the columns of the
    // jacobian matrix. If parameter_blocks is empty, then it is
    // assumed to be equal to vector containing ALL the parameter
    // blocks.  Generally speaking the parameter blocks will occur in
    // the order in which they were added to the problem. But, this
    // may change if the user removes any parameter blocks from the
    // problem.
    //
    // NOTE: This vector should contain the same pointers as the ones
    // used to add parameter blocks to the Problem. These parameter
    // block should NOT point to new memory locations. Bad things will
    // happen otherwise.
    std::vector<double*> parameter_blocks;

    // The set of residual blocks to evaluate. This vector determines
    // the order in which the residuals occur, and how the rows of the
    // jacobian are ordered. If residual_blocks is empty, then it is
    // assumed to be equal to the vector containing ALL the residual
    // blocks. Generally speaking the residual blocks will occur in
    // the order in which they were added to the problem. But, this
    // may change if the user removes any residual blocks from the
    // problem.
    std::vector<ResidualBlockId> residual_blocks;

    // Even though the residual blocks in the problem may contain loss
    // functions, setting apply_loss_function to false will turn off
    // the application of the loss function to the output of the cost
    // function. This is of use for example if the user wishes to
    // analyse the solution quality by studying the distribution of
    // residuals before and after the solve.
    bool apply_loss_function = true;

    int num_threads = 1;
  };

  // Evaluate Problem. Any of the output pointers can be nullptr. Which
  // residual blocks and parameter blocks are used is controlled by
  // the EvaluateOptions struct above.
  //
  // Note 1: The evaluation will use the values stored in the memory
  // locations pointed to by the parameter block pointers used at the
  // time of the construction of the problem. i.e.,
  //
  //   Problem problem;
  //   double x = 1;
  //   problem.AddResidualBlock(new MyCostFunction, nullptr, &x);
  //
  //   double cost = 0.0;
  //   problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr);
  //
  // The cost is evaluated at x = 1. If you wish to evaluate the
  // problem at x = 2, then
  //
  //    x = 2;
  //    problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr);
  //
  // is the way to do so.
  //
  // Note 2: If no local parameterizations are used, then the size of
  // the gradient vector (and the number of columns in the jacobian)
  // is the sum of the sizes of all the parameter blocks. If a
  // parameter block has a local parameterization, then it contributes
  // "LocalSize" entries to the gradient vector (and the number of
  // columns in the jacobian).
  //
  // Note 3: This function cannot be called while the problem is being
  // solved, for example it cannot be called from an IterationCallback
  // at the end of an iteration during a solve.
  bool Evaluate(const EvaluateOptions& options,
                double* cost,
                std::vector<double>* residuals,
                std::vector<double>* gradient,
                CRSMatrix* jacobian);

  // Evaluates the residual block, storing the scalar cost in *cost,
  // the residual components in *residuals, and the jacobians between
  // the parameters and residuals in jacobians[i], in row-major order.
  //
  // If residuals is nullptr, the residuals are not computed.
  //
  // If jacobians is nullptr, no Jacobians are computed. If
  // jacobians[i] is nullptr, then the Jacobian for that parameter
  // block is not computed.
  //
  // It is not okay to request the Jacobian w.r.t a parameter block
  // that is constant.
  //
  // The return value indicates the success or failure. Even if the
  // function returns false, the caller should expect the output
  // memory locations to have been modified.
  //
  // The returned cost and jacobians have had robustification and
  // local parameterizations applied already; for example, the
  // jacobian for a 4-dimensional quaternion parameter using the
  // "QuaternionParameterization" is num_residuals by 3 instead of
  // num_residuals by 4.
  //
  // apply_loss_function as the name implies allows the user to switch
  // the application of the loss function on and off.
  //
  // TODO(sameeragarwal): Clarify interaction with IterationCallback
  // once that cleanup is done.
  bool EvaluateResidualBlock(ResidualBlockId residual_block_id,
                             bool apply_loss_function,
                             double* cost,
                             double* residuals,
                             double** jacobians) const;
 private:
  friend class Solver;
  friend class Covariance;
  std::unique_ptr<internal::ProblemImpl> impl_;
};

}  // namespace ceres

#include "ceres/internal/reenable_warnings.h"

#endif  // CERES_PUBLIC_PROBLEM_H_