| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2021 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/export.h" |
| #include "ceres/internal/port.h" |
| #include "ceres/types.h" |
| #include "glog/logging.h" |
| |
| namespace ceres { |
| |
| class CostFunction; |
| class EvaluationCallback; |
| class LossFunction; |
| class Manifold; |
| 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. |
| using ResidualBlockId = internal::ResidualBlock*; |
| |
| // 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 CostFunctions, |
| // LossFunctions, and Manifolds passed into the Problem. |
| // |
| // If set to TAKE_OWNERSHIP, then the problem object will delete the |
| // corresponding object on destruction. The destructor is careful to delete |
| // the pointers only once, since sharing objects is allowed. |
| Ownership cost_function_ownership = TAKE_OWNERSHIP; |
| Ownership loss_function_ownership = TAKE_OWNERSHIP; |
| Ownership manifold_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 two-fold: 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; |
| |
| // Using this callback interface, Ceres can notify you when it is about to |
| // evaluate the residuals or jacobians. With the callback, you can share |
| // computation between residual blocks by doing the shared computation in |
| // EvaluationCallback::PrepareForEvaluation() before Ceres calls |
| // CostFunction::Evaluate(). It also enables caching results between a pure |
| // residual evaluation and a residual & jacobian evaluation. |
| // |
| // Problem DOES NOT take ownership of the callback. |
| // |
| // NOTE: Evaluation callbacks are incompatible with inner iterations. So |
| // calling Solve with Solver::Options::use_inner_iterations = true on a |
| // Problem with a non-null evaluation callback is an error. |
| EvaluationCallback* evaluation_callback = nullptr; |
| }; |
| |
| // The default constructor is equivalent to the invocation |
| // Problem(Problem::Options()). |
| Problem(); |
| explicit Problem(const Options& options); |
| Problem(Problem&&); |
| Problem& operator=(Problem&&); |
| |
| Problem(const Problem&) = delete; |
| Problem& 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 (See Problem::Options to override this behaviour). |
| // 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 will result in a crash. |
| void AddParameterBlock(double* values, int size); |
| |
| // Add a parameter block with appropriate size and Manifold to the |
| // problem. It is okay for manifold to be nullptr. |
| // |
| // Repeated calls with the same arguments are ignored. Repeated calls |
| // with the same double pointer but a different size results in a crash |
| // (unless Solver::Options::disable_all_safety_checks is set to true). |
| // |
| // Repeated calls with the same double pointer and size but different Manifold |
| // is equivalent to calling SetManifold(manifold), i.e., any previously |
| // associated Manifold object will be replaced with the manifold. |
| void AddParameterBlock(double* values, int size, Manifold* manifold); |
| |
| // Remove a parameter block from the problem. The Manifold 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. A |
| // parameter block may be set constant in two ways: either by calling |
| // SetParameterBlockConstant or by associating a Manifold with a zero |
| // dimensional tangent space with it. |
| bool IsParameterBlockConstant(const double* values) const; |
| |
| // Set the Manifold for the parameter block. Calling SetManifold with nullptr |
| // will clear any previously set Manifold for the parameter block. |
| // |
| // Repeated calls will result in any previously associated Manifold object to |
| // be replaced with the manifold. |
| // |
| // The manifold is owned by the Problem by default (See Problem::Options to |
| // override this behaviour). |
| // |
| // It is acceptable to set the same Manifold for multiple parameter blocks. |
| void SetManifold(double* values, Manifold* manifold); |
| |
| // Get the Manifold object associated with this parameter block. |
| // |
| // If there is no Manifold object associated then nullptr is returned. |
| const Manifold* GetManifold(const double* values) const; |
| |
| // Returns true if a Manifold is associated with this parameter block, false |
| // otherwise. |
| bool HasManifold(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 dimension of the tangent space of the Manifold for the parameter block. |
| // If there is no Manifold associated with this parameter block, then |
| // ParameterBlockTangentSize = ParameterBlockSize. |
| int ParameterBlockTangentSize(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 Manifolds 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 Manifold, then it contributes |
| // "TangentSize" 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. |
| // |
| // Note 4: If an EvaluationCallback is associated with the problem, then its |
| // PrepareForEvaluation method will be called every time this method is called |
| // with new_point = true. |
| 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 Manifold |
| // 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. |
| // |
| // If an EvaluationCallback is associated with the problem, then its |
| // PrepareForEvaluation method will be called every time this method is called |
| // with new_point = true. This conservatively assumes that the user may have |
| // changed the parameter values since the previous call to evaluate / solve. |
| // For improved efficiency, and only if you know that the parameter values |
| // have not changed between calls, see |
| // EvaluateResidualBlockAssumingParametersUnchanged(). |
| bool EvaluateResidualBlock(ResidualBlockId residual_block_id, |
| bool apply_loss_function, |
| double* cost, |
| double* residuals, |
| double** jacobians) const; |
| |
| // Same as EvaluateResidualBlock except that if an EvaluationCallback is |
| // associated with the problem, then its PrepareForEvaluation method will be |
| // called every time this method is called with new_point = false. |
| // |
| // This means, if an EvaluationCallback is associated with the problem then it |
| // is the user's responsibility to call PrepareForEvaluation before calling |
| // this method if necessary, i.e. iff the parameter values have been changed |
| // since the last call to evaluate / solve.' |
| // |
| // This is because, as the name implies, we assume that the parameter blocks |
| // did not change since the last time PrepareForEvaluation was called (via |
| // Solve, Evaluate or EvaluateResidualBlock). |
| bool EvaluateResidualBlockAssumingParametersUnchanged( |
| ResidualBlockId residual_block_id, |
| bool apply_loss_function, |
| double* cost, |
| double* residuals, |
| double** jacobians) const; |
| |
| // Returns reference to the options with which the Problem was constructed. |
| const Options & options() const; |
| |
| // Returns pointer to Problem implementation |
| internal::ProblemImpl* mutable_impl(); |
| |
| private: |
| std::unique_ptr<internal::ProblemImpl> impl_; |
| }; |
| |
| } // namespace ceres |
| |
| #include "ceres/internal/reenable_warnings.h" |
| |
| #endif // CERES_PUBLIC_PROBLEM_H_ |