Home | History | Annotate | Download | only in ceres
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2013 Google Inc. All rights reserved.
      3 // http://code.google.com/p/ceres-solver/
      4 //
      5 // Redistribution and use in source and binary forms, with or without
      6 // modification, are permitted provided that the following conditions are met:
      7 //
      8 // * Redistributions of source code must retain the above copyright notice,
      9 //   this list of conditions and the following disclaimer.
     10 // * Redistributions in binary form must reproduce the above copyright notice,
     11 //   this list of conditions and the following disclaimer in the documentation
     12 //   and/or other materials provided with the distribution.
     13 // * Neither the name of Google Inc. nor the names of its contributors may be
     14 //   used to endorse or promote products derived from this software without
     15 //   specific prior written permission.
     16 //
     17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
     18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
     19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
     20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
     21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
     23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
     24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
     25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
     26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     27 // POSSIBILITY OF SUCH DAMAGE.
     28 //
     29 // Author: sameeragarwal (at) google.com (Sameer Agarwal)
     30 //         keir (at) google.com (Keir Mierle)
     31 //
     32 // The Problem object is used to build and hold least squares problems.
     33 
     34 #ifndef CERES_PUBLIC_PROBLEM_H_
     35 #define CERES_PUBLIC_PROBLEM_H_
     36 
     37 #include <cstddef>
     38 #include <map>
     39 #include <set>
     40 #include <vector>
     41 
     42 #include "glog/logging.h"
     43 #include "ceres/internal/macros.h"
     44 #include "ceres/internal/port.h"
     45 #include "ceres/internal/scoped_ptr.h"
     46 #include "ceres/types.h"
     47 #include "ceres/internal/disable_warnings.h"
     48 
     49 
     50 namespace ceres {
     51 
     52 class CostFunction;
     53 class LossFunction;
     54 class LocalParameterization;
     55 class Solver;
     56 struct CRSMatrix;
     57 
     58 namespace internal {
     59 class Preprocessor;
     60 class ProblemImpl;
     61 class ParameterBlock;
     62 class ResidualBlock;
     63 }  // namespace internal
     64 
     65 // A ResidualBlockId is an opaque handle clients can use to remove residual
     66 // blocks from a Problem after adding them.
     67 typedef internal::ResidualBlock* ResidualBlockId;
     68 
     69 // A class to represent non-linear least squares problems. Such
     70 // problems have a cost function that is a sum of error terms (known
     71 // as "residuals"), where each residual is a function of some subset
     72 // of the parameters. The cost function takes the form
     73 //
     74 //    N    1
     75 //   SUM  --- loss( || r_i1, r_i2,..., r_ik ||^2  ),
     76 //   i=1   2
     77 //
     78 // where
     79 //
     80 //   r_ij     is residual number i, component j; the residual is a
     81 //            function of some subset of the parameters x1...xk. For
     82 //            example, in a structure from motion problem a residual
     83 //            might be the difference between a measured point in an
     84 //            image and the reprojected position for the matching
     85 //            camera, point pair. The residual would have two
     86 //            components, error in x and error in y.
     87 //
     88 //   loss(y)  is the loss function; for example, squared error or
     89 //            Huber L1 loss. If loss(y) = y, then the cost function is
     90 //            non-robustified least squares.
     91 //
     92 // This class is specifically designed to address the important subset
     93 // of "sparse" least squares problems, where each component of the
     94 // residual depends only on a small number number of parameters, even
     95 // though the total number of residuals and parameters may be very
     96 // large. This property affords tremendous gains in scale, allowing
     97 // efficient solving of large problems that are otherwise
     98 // inaccessible.
     99 //
    100 // The canonical example of a sparse least squares problem is
    101 // "structure-from-motion" (SFM), where the parameters are points and
    102 // cameras, and residuals are reprojection errors. Typically a single
    103 // residual will depend only on 9 parameters (3 for the point, 6 for
    104 // the camera).
    105 //
    106 // To create a least squares problem, use the AddResidualBlock() and
    107 // AddParameterBlock() methods, documented below. Here is an example least
    108 // squares problem containing 3 parameter blocks of sizes 3, 4 and 5
    109 // respectively and two residual terms of size 2 and 6:
    110 //
    111 //   double x1[] = { 1.0, 2.0, 3.0 };
    112 //   double x2[] = { 1.0, 2.0, 3.0, 5.0 };
    113 //   double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 };
    114 //
    115 //   Problem problem;
    116 //
    117 //   problem.AddResidualBlock(new MyUnaryCostFunction(...), x1);
    118 //   problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3);
    119 //
    120 // Please see cost_function.h for details of the CostFunction object.
    121 class CERES_EXPORT Problem {
    122  public:
    123   struct CERES_EXPORT Options {
    124     Options()
    125         : cost_function_ownership(TAKE_OWNERSHIP),
    126           loss_function_ownership(TAKE_OWNERSHIP),
    127           local_parameterization_ownership(TAKE_OWNERSHIP),
    128           enable_fast_removal(false),
    129           disable_all_safety_checks(false) {}
    130 
    131     // These flags control whether the Problem object owns the cost
    132     // functions, loss functions, and parameterizations passed into
    133     // the Problem. If set to TAKE_OWNERSHIP, then the problem object
    134     // will delete the corresponding cost or loss functions on
    135     // destruction. The destructor is careful to delete the pointers
    136     // only once, since sharing cost/loss/parameterizations is
    137     // allowed.
    138     Ownership cost_function_ownership;
    139     Ownership loss_function_ownership;
    140     Ownership local_parameterization_ownership;
    141 
    142     // If true, trades memory for faster RemoveResidualBlock() and
    143     // RemoveParameterBlock() operations.
    144     //
    145     // By default, RemoveParameterBlock() and RemoveResidualBlock() take time
    146     // proportional to the size of the entire problem.  If you only ever remove
    147     // parameters or residuals from the problem occassionally, this might be
    148     // acceptable.  However, if you have memory to spare, enable this option to
    149     // make RemoveParameterBlock() take time proportional to the number of
    150     // residual blocks that depend on it, and RemoveResidualBlock() take (on
    151     // average) constant time.
    152     //
    153     // The increase in memory usage is twofold: an additonal hash set per
    154     // parameter block containing all the residuals that depend on the parameter
    155     // block; and a hash set in the problem containing all residuals.
    156     bool enable_fast_removal;
    157 
    158     // By default, Ceres performs a variety of safety checks when constructing
    159     // the problem. There is a small but measurable performance penalty to
    160     // these checks, typically around 5% of construction time. If you are sure
    161     // your problem construction is correct, and 5% of the problem construction
    162     // time is truly an overhead you want to avoid, then you can set
    163     // disable_all_safety_checks to true.
    164     //
    165     // WARNING: Do not set this to true, unless you are absolutely sure of what
    166     // you are doing.
    167     bool disable_all_safety_checks;
    168   };
    169 
    170   // The default constructor is equivalent to the
    171   // invocation Problem(Problem::Options()).
    172   Problem();
    173   explicit Problem(const Options& options);
    174 
    175   ~Problem();
    176 
    177   // Add a residual block to the overall cost function. The cost
    178   // function carries with it information about the sizes of the
    179   // parameter blocks it expects. The function checks that these match
    180   // the sizes of the parameter blocks listed in parameter_blocks. The
    181   // program aborts if a mismatch is detected. loss_function can be
    182   // NULL, in which case the cost of the term is just the squared norm
    183   // of the residuals.
    184   //
    185   // The user has the option of explicitly adding the parameter blocks
    186   // using AddParameterBlock. This causes additional correctness
    187   // checking; however, AddResidualBlock implicitly adds the parameter
    188   // blocks if they are not present, so calling AddParameterBlock
    189   // explicitly is not required.
    190   //
    191   // The Problem object by default takes ownership of the
    192   // cost_function and loss_function pointers. These objects remain
    193   // live for the life of the Problem object. If the user wishes to
    194   // keep control over the destruction of these objects, then they can
    195   // do this by setting the corresponding enums in the Options struct.
    196   //
    197   // Note: Even though the Problem takes ownership of cost_function
    198   // and loss_function, it does not preclude the user from re-using
    199   // them in another residual block. The destructor takes care to call
    200   // delete on each cost_function or loss_function pointer only once,
    201   // regardless of how many residual blocks refer to them.
    202   //
    203   // Example usage:
    204   //
    205   //   double x1[] = {1.0, 2.0, 3.0};
    206   //   double x2[] = {1.0, 2.0, 5.0, 6.0};
    207   //   double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0};
    208   //
    209   //   Problem problem;
    210   //
    211   //   problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1);
    212   //   problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1);
    213   //
    214   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    215                                    LossFunction* loss_function,
    216                                    const vector<double*>& parameter_blocks);
    217 
    218   // Convenience methods for adding residuals with a small number of
    219   // parameters. This is the common case. Instead of specifying the
    220   // parameter block arguments as a vector, list them as pointers.
    221   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    222                                    LossFunction* loss_function,
    223                                    double* x0);
    224   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    225                                    LossFunction* loss_function,
    226                                    double* x0, double* x1);
    227   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    228                                    LossFunction* loss_function,
    229                                    double* x0, double* x1, double* x2);
    230   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    231                                    LossFunction* loss_function,
    232                                    double* x0, double* x1, double* x2,
    233                                    double* x3);
    234   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    235                                    LossFunction* loss_function,
    236                                    double* x0, double* x1, double* x2,
    237                                    double* x3, double* x4);
    238   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    239                                    LossFunction* loss_function,
    240                                    double* x0, double* x1, double* x2,
    241                                    double* x3, double* x4, double* x5);
    242   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    243                                    LossFunction* loss_function,
    244                                    double* x0, double* x1, double* x2,
    245                                    double* x3, double* x4, double* x5,
    246                                    double* x6);
    247   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    248                                    LossFunction* loss_function,
    249                                    double* x0, double* x1, double* x2,
    250                                    double* x3, double* x4, double* x5,
    251                                    double* x6, double* x7);
    252   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    253                                    LossFunction* loss_function,
    254                                    double* x0, double* x1, double* x2,
    255                                    double* x3, double* x4, double* x5,
    256                                    double* x6, double* x7, double* x8);
    257   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
    258                                    LossFunction* loss_function,
    259                                    double* x0, double* x1, double* x2,
    260                                    double* x3, double* x4, double* x5,
    261                                    double* x6, double* x7, double* x8,
    262                                    double* x9);
    263 
    264   // Add a parameter block with appropriate size to the problem.
    265   // Repeated calls with the same arguments are ignored. Repeated
    266   // calls with the same double pointer but a different size results
    267   // in undefined behaviour.
    268   void AddParameterBlock(double* values, int size);
    269 
    270   // Add a parameter block with appropriate size and parameterization
    271   // to the problem. Repeated calls with the same arguments are
    272   // ignored. Repeated calls with the same double pointer but a
    273   // different size results in undefined behaviour.
    274   void AddParameterBlock(double* values,
    275                          int size,
    276                          LocalParameterization* local_parameterization);
    277 
    278   // Remove a parameter block from the problem. The parameterization of the
    279   // parameter block, if it exists, will persist until the deletion of the
    280   // problem (similar to cost/loss functions in residual block removal). Any
    281   // residual blocks that depend on the parameter are also removed, as
    282   // described above in RemoveResidualBlock().
    283   //
    284   // If Problem::Options::enable_fast_removal is true, then the
    285   // removal is fast (almost constant time). Otherwise, removing a parameter
    286   // block will incur a scan of the entire Problem object.
    287   //
    288   // WARNING: Removing a residual or parameter block will destroy the implicit
    289   // ordering, rendering the jacobian or residuals returned from the solver
    290   // uninterpretable. If you depend on the evaluated jacobian, do not use
    291   // remove! This may change in a future release.
    292   void RemoveParameterBlock(double* values);
    293 
    294   // Remove a residual block from the problem. Any parameters that the residual
    295   // block depends on are not removed. The cost and loss functions for the
    296   // residual block will not get deleted immediately; won't happen until the
    297   // problem itself is deleted.
    298   //
    299   // WARNING: Removing a residual or parameter block will destroy the implicit
    300   // ordering, rendering the jacobian or residuals returned from the solver
    301   // uninterpretable. If you depend on the evaluated jacobian, do not use
    302   // remove! This may change in a future release.
    303   void RemoveResidualBlock(ResidualBlockId residual_block);
    304 
    305   // Hold the indicated parameter block constant during optimization.
    306   void SetParameterBlockConstant(double* values);
    307 
    308   // Allow the indicated parameter block to vary during optimization.
    309   void SetParameterBlockVariable(double* values);
    310 
    311   // Set the local parameterization for one of the parameter blocks.
    312   // The local_parameterization is owned by the Problem by default. It
    313   // is acceptable to set the same parameterization for multiple
    314   // parameters; the destructor is careful to delete local
    315   // parameterizations only once. The local parameterization can only
    316   // be set once per parameter, and cannot be changed once set.
    317   void SetParameterization(double* values,
    318                            LocalParameterization* local_parameterization);
    319 
    320   // Get the local parameterization object associated with this
    321   // parameter block. If there is no parameterization object
    322   // associated then NULL is returned.
    323   const LocalParameterization* GetParameterization(double* values) const;
    324 
    325   // Set the lower/upper bound for the parameter with position "index".
    326   void SetParameterLowerBound(double* values, int index, double lower_bound);
    327   void SetParameterUpperBound(double* values, int index, double upper_bound);
    328 
    329   // Number of parameter blocks in the problem. Always equals
    330   // parameter_blocks().size() and parameter_block_sizes().size().
    331   int NumParameterBlocks() const;
    332 
    333   // The size of the parameter vector obtained by summing over the
    334   // sizes of all the parameter blocks.
    335   int NumParameters() const;
    336 
    337   // Number of residual blocks in the problem. Always equals
    338   // residual_blocks().size().
    339   int NumResidualBlocks() const;
    340 
    341   // The size of the residual vector obtained by summing over the
    342   // sizes of all of the residual blocks.
    343   int NumResiduals() const;
    344 
    345   // The size of the parameter block.
    346   int ParameterBlockSize(const double* values) const;
    347 
    348   // The size of local parameterization for the parameter block. If
    349   // there is no local parameterization associated with this parameter
    350   // block, then ParameterBlockLocalSize = ParameterBlockSize.
    351   int ParameterBlockLocalSize(const double* values) const;
    352 
    353   // Is the given parameter block present in this problem or not?
    354   bool HasParameterBlock(const double* values) const;
    355 
    356   // Fills the passed parameter_blocks vector with pointers to the
    357   // parameter blocks currently in the problem. After this call,
    358   // parameter_block.size() == NumParameterBlocks.
    359   void GetParameterBlocks(vector<double*>* parameter_blocks) const;
    360 
    361   // Fills the passed residual_blocks vector with pointers to the
    362   // residual blocks currently in the problem. After this call,
    363   // residual_blocks.size() == NumResidualBlocks.
    364   void GetResidualBlocks(vector<ResidualBlockId>* residual_blocks) const;
    365 
    366   // Get all the parameter blocks that depend on the given residual block.
    367   void GetParameterBlocksForResidualBlock(
    368       const ResidualBlockId residual_block,
    369       vector<double*>* parameter_blocks) const;
    370 
    371   // Get all the residual blocks that depend on the given parameter block.
    372   //
    373   // If Problem::Options::enable_fast_removal is true, then
    374   // getting the residual blocks is fast and depends only on the number of
    375   // residual blocks. Otherwise, getting the residual blocks for a parameter
    376   // block will incur a scan of the entire Problem object.
    377   void GetResidualBlocksForParameterBlock(
    378       const double* values,
    379       vector<ResidualBlockId>* residual_blocks) const;
    380 
    381   // Options struct to control Problem::Evaluate.
    382   struct EvaluateOptions {
    383     EvaluateOptions()
    384         : apply_loss_function(true),
    385           num_threads(1) {
    386     }
    387 
    388     // The set of parameter blocks for which evaluation should be
    389     // performed. This vector determines the order that parameter
    390     // blocks occur in the gradient vector and in the columns of the
    391     // jacobian matrix. If parameter_blocks is empty, then it is
    392     // assumed to be equal to vector containing ALL the parameter
    393     // blocks.  Generally speaking the parameter blocks will occur in
    394     // the order in which they were added to the problem. But, this
    395     // may change if the user removes any parameter blocks from the
    396     // problem.
    397     //
    398     // NOTE: This vector should contain the same pointers as the ones
    399     // used to add parameter blocks to the Problem. These parameter
    400     // block should NOT point to new memory locations. Bad things will
    401     // happen otherwise.
    402     vector<double*> parameter_blocks;
    403 
    404     // The set of residual blocks to evaluate. This vector determines
    405     // the order in which the residuals occur, and how the rows of the
    406     // jacobian are ordered. If residual_blocks is empty, then it is
    407     // assumed to be equal to the vector containing all the residual
    408     // blocks. If this vector is empty, then it is assumed to be equal
    409     // to a vector containing ALL the residual blocks. Generally
    410     // speaking the residual blocks will occur in the order in which
    411     // they were added to the problem. But, this may change if the
    412     // user removes any residual blocks from the problem.
    413     vector<ResidualBlockId> residual_blocks;
    414 
    415     // Even though the residual blocks in the problem may contain loss
    416     // functions, setting apply_loss_function to false will turn off
    417     // the application of the loss function to the output of the cost
    418     // function. This is of use for example if the user wishes to
    419     // analyse the solution quality by studying the distribution of
    420     // residuals before and after the solve.
    421     bool apply_loss_function;
    422 
    423     int num_threads;
    424   };
    425 
    426   // Evaluate Problem. Any of the output pointers can be NULL. Which
    427   // residual blocks and parameter blocks are used is controlled by
    428   // the EvaluateOptions struct above.
    429   //
    430   // Note 1: The evaluation will use the values stored in the memory
    431   // locations pointed to by the parameter block pointers used at the
    432   // time of the construction of the problem. i.e.,
    433   //
    434   //   Problem problem;
    435   //   double x = 1;
    436   //   problem.AddResidualBlock(new MyCostFunction, NULL, &x);
    437   //
    438   //   double cost = 0.0;
    439   //   problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
    440   //
    441   // The cost is evaluated at x = 1. If you wish to evaluate the
    442   // problem at x = 2, then
    443   //
    444   //    x = 2;
    445   //    problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
    446   //
    447   // is the way to do so.
    448   //
    449   // Note 2: If no local parameterizations are used, then the size of
    450   // the gradient vector (and the number of columns in the jacobian)
    451   // is the sum of the sizes of all the parameter blocks. If a
    452   // parameter block has a local parameterization, then it contributes
    453   // "LocalSize" entries to the gradient vector (and the number of
    454   // columns in the jacobian).
    455   bool Evaluate(const EvaluateOptions& options,
    456                 double* cost,
    457                 vector<double>* residuals,
    458                 vector<double>* gradient,
    459                 CRSMatrix* jacobian);
    460 
    461  private:
    462   friend class Solver;
    463   friend class Covariance;
    464   internal::scoped_ptr<internal::ProblemImpl> problem_impl_;
    465   CERES_DISALLOW_COPY_AND_ASSIGN(Problem);
    466 };
    467 
    468 }  // namespace ceres
    469 
    470 #include "ceres/internal/reenable_warnings.h"
    471 
    472 #endif  // CERES_PUBLIC_PROBLEM_H_
    473