MYNT-EYE-S-SDK/3rdparty/ceres-solver-1.11.0/include/ceres/problem.h

482 lines
22 KiB
C
Raw Normal View History

2019-01-03 10:25:18 +02:00
// 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 <cstddef>
#include <map>
#include <set>
#include <vector>
#include "glog/logging.h"
#include "ceres/internal/macros.h"
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"
#include "ceres/internal/disable_warnings.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(...), x1);
// problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3);
//
// Please see cost_function.h for details of the CostFunction object.
class CERES_EXPORT Problem {
public:
struct CERES_EXPORT Options {
Options()
: cost_function_ownership(TAKE_OWNERSHIP),
loss_function_ownership(TAKE_OWNERSHIP),
local_parameterization_ownership(TAKE_OWNERSHIP),
enable_fast_removal(false),
disable_all_safety_checks(false) {}
// 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;
Ownership loss_function_ownership;
Ownership local_parameterization_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 occassionally, 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 additonal 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;
// 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;
};
// The default constructor is equivalent to the
// invocation Problem(Problem::Options()).
Problem();
explicit Problem(const Options& options);
~Problem();
// Add a residual block to the overall cost function. The cost
// function carries with it 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
// NULL, 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(...), NULL, x1);
// problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1);
//
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,
LossFunction* loss_function,
const std::vector<double*>& parameter_blocks);
// Convenience methods for adding residuals with a small number of
// parameters. This is the common case. Instead of specifying the
// parameter block arguments as a vector, list them as pointers.
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7, double* x8);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7, double* x8,
double* x9);
// 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(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(double* values);
// Allow the indicated parameter block to vary during optimization.
void SetParameterBlockVariable(double* values);
// 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 NULL is returned.
const LocalParameterization* GetParameterization(double* values) const;
// Set the lower/upper bound for the parameter with position "index".
void SetParameterLowerBound(double* values, int index, double lower_bound);
void SetParameterUpperBound(double* values, int index, double upper_bound);
// 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 NULL
// 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 {
EvaluateOptions()
: apply_loss_function(true),
num_threads(1) {
}
// 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;
int num_threads;
};
// Evaluate Problem. Any of the output pointers can be NULL. 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, NULL, &x);
//
// double cost = 0.0;
// problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
//
// 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, NULL, NULL, NULL);
//
// 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).
bool Evaluate(const EvaluateOptions& options,
double* cost,
std::vector<double>* residuals,
std::vector<double>* gradient,
CRSMatrix* jacobian);
private:
friend class Solver;
friend class Covariance;
internal::scoped_ptr<internal::ProblemImpl> problem_impl_;
CERES_DISALLOW_COPY_AND_ASSIGN(Problem);
};
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
#endif // CERES_PUBLIC_PROBLEM_H_