Vehicle-cpp/zjw/ceres-solver-master/include/ceres/covariance.h

// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2023 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)

#ifndef CERES_PUBLIC_COVARIANCE_H_
#define CERES_PUBLIC_COVARIANCE_H_

#include <memory>
#include <utility>
#include <vector>

#include "ceres/internal/config.h"
#include "ceres/internal/disable_warnings.h"
#include "ceres/internal/export.h"
#include "ceres/types.h"

namespace ceres {

class Problem;

namespace internal {
class CovarianceImpl;
}  // namespace internal

// WARNING
// =======
// It is very easy to use this class incorrectly without understanding
// the underlying mathematics. Please read and understand the
// documentation completely before attempting to use it.
//
//
// This class allows the user to evaluate the covariance for a
// non-linear least squares problem and provides random access to its
// blocks
//
// Background
// ==========
// One way to assess the quality of the solution returned by a
// non-linear least squares solver is to analyze the covariance of the
// solution.
//
// Let us consider the non-linear regression problem
//
//   y = f(x) + N(0, I)
//
// i.e., the observation y is a random non-linear function of the
// independent variable x with mean f(x) and identity covariance. Then
// the maximum likelihood estimate of x given observations y is the
// solution to the non-linear least squares problem:
//
//  x* = arg min_x |f(x) - y|^2
//
// And the covariance of x* is given by
//
//  C(x*) = inverse[J'(x*)J(x*)]
//
// Here J(x*) is the Jacobian of f at x*. The above formula assumes
// that J(x*) has full column rank.
//
// If J(x*) is rank deficient, then the covariance matrix C(x*) is
// also rank deficient and is given by
//
//  C(x*) =  pseudoinverse[J'(x*)J(x*)]
//
// Note that in the above, we assumed that the covariance
// matrix for y was identity. This is an important assumption. If this
// is not the case and we have
//
//  y = f(x) + N(0, S)
//
// Where S is a positive semi-definite matrix denoting the covariance
// of y, then the maximum likelihood problem to be solved is
//
//  x* = arg min_x f'(x) inverse[S] f(x)
//
// and the corresponding covariance estimate of x* is given by
//
//  C(x*) = inverse[J'(x*) inverse[S] J(x*)]
//
// So, if it is the case that the observations being fitted to have a
// covariance matrix not equal to identity, then it is the user's
// responsibility that the corresponding cost functions are correctly
// scaled, e.g. in the above case the cost function for this problem
// should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2}
// is the inverse square root of the covariance matrix S.
//
// This class allows the user to evaluate the covariance for a
// non-linear least squares problem and provides random access to its
// blocks. The computation assumes that the CostFunctions compute
// residuals such that their covariance is identity.
//
// Since the computation of the covariance matrix requires computing
// the inverse of a potentially large matrix, this can involve a
// rather large amount of time and memory. However, it is usually the
// case that the user is only interested in a small part of the
// covariance matrix. Quite often just the block diagonal. This class
// allows the user to specify the parts of the covariance matrix that
// she is interested in and then uses this information to only compute
// and store those parts of the covariance matrix.
//
// Rank of the Jacobian
// --------------------
// As we noted above, if the jacobian is rank deficient, then the
// inverse of J'J is not defined and instead a pseudo inverse needs to
// be computed.
//
// The rank deficiency in J can be structural -- columns which are
// always known to be zero or numerical -- depending on the exact
// values in the Jacobian.
//
// Structural rank deficiency occurs when the problem contains
// parameter blocks that are constant. This class correctly handles
// structural rank deficiency like that.
//
// Numerical rank deficiency, where the rank of the matrix cannot be
// predicted by its sparsity structure and requires looking at its
// numerical values is more complicated. Here again there are two
// cases.
//
//   a. The rank deficiency arises from overparameterization. e.g., a
//   four dimensional quaternion used to parameterize SO(3), which is
//   a three dimensional manifold. In cases like this, the user should
//   use an appropriate Manifold. Not only will this lead
//   to better numerical behaviour of the Solver, it will also expose
//   the rank deficiency to the Covariance object so that it can
//   handle it correctly.
//
//   b. More general numerical rank deficiency in the Jacobian
//   requires the computation of the so called Singular Value
//   Decomposition (SVD) of J'J. We do not know how to do this for
//   large sparse matrices efficiently. For small and moderate sized
//   problems this is done using dense linear algebra.
//
// Gauge Invariance
// ----------------
// In structure from motion (3D reconstruction) problems, the
// reconstruction is ambiguous up to a similarity transform. This is
// known as a Gauge Ambiguity. Handling Gauges correctly requires the
// use of SVD or custom inversion algorithms. For small problems the
// user can use the dense algorithm. For more details see
//
// Ken-ichi Kanatani, Daniel D. Morris: Gauges and gauge
// transformations for uncertainty description of geometric structure
// with indeterminacy. IEEE Transactions on Information Theory 47(5):
// 2017-2028 (2001)
//
// Example Usage
// =============
//
//  double x[3];
//  double y[2];
//
//  Problem problem;
//  problem.AddParameterBlock(x, 3);
//  problem.AddParameterBlock(y, 2);
//  <Build Problem>
//  <Solve Problem>
//
//  Covariance::Options options;
//  Covariance covariance(options);
//
//  std::vector<std::pair<const double*, const double*>> covariance_blocks;
//  covariance_blocks.push_back(make_pair(x, x));
//  covariance_blocks.push_back(make_pair(y, y));
//  covariance_blocks.push_back(make_pair(x, y));
//
//  CHECK(covariance.Compute(covariance_blocks, &problem));
//
//  double covariance_xx[3 * 3];
//  double covariance_yy[2 * 2];
//  double covariance_xy[3 * 2];
//  covariance.GetCovarianceBlock(x, x, covariance_xx)
//  covariance.GetCovarianceBlock(y, y, covariance_yy)
//  covariance.GetCovarianceBlock(x, y, covariance_xy)
//
class CERES_EXPORT Covariance {
 public:
  struct CERES_EXPORT Options {
    // Sparse linear algebra library to use when a sparse matrix
    // factorization is being used to compute the covariance matrix.
    //
    // Currently this only applies to SPARSE_QR.
    SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type =
#if !defined(CERES_NO_SUITESPARSE)
        SUITE_SPARSE;
#else
        // Eigen's QR factorization is always available.
        EIGEN_SPARSE;
#endif

    // Ceres supports two different algorithms for covariance
    // estimation, which represent different tradeoffs in speed,
    // accuracy and reliability.
    //
    // 1. DENSE_SVD uses Eigen's JacobiSVD to perform the
    //    computations. It computes the singular value decomposition
    //
    //      U * D * V' = J
    //
    //    and then uses it to compute the pseudo inverse of J'J as
    //
    //      pseudoinverse[J'J] = V * pseudoinverse[D^2] * V'
    //
    //    It is an accurate but slow method and should only be used
    //    for small to moderate sized problems. It can handle
    //    full-rank as well as rank deficient Jacobians.
    //
    // 2. SPARSE_QR uses the sparse QR factorization algorithm
    //    to compute the decomposition
    //
    //      Q * R = J
    //
    //    [J'J]^-1 = [R'*R]^-1
    //
    // SPARSE_QR is not capable of computing the covariance if the
    // Jacobian is rank deficient. Depending on the value of
    // Covariance::Options::sparse_linear_algebra_library_type, either
    // Eigen's Sparse QR factorization algorithm will be used or
    // SuiteSparse's high performance SuiteSparseQR algorithm will be
    // used.
    CovarianceAlgorithmType algorithm_type = SPARSE_QR;

    // During QR factorization, if a column with Euclidean norm less
    // than column_pivot_threshold is encountered it is treated as
    // zero.
    //
    // If column_pivot_threshold < 0, then an automatic default value
    // of 20*(m+n)*eps*sqrt(max(diag(J’*J))) is used. Here m and n are
    // the number of rows and columns of the Jacobian (J)
    // respectively.
    //
    // This is an advanced option meant for users who know enough
    // about their Jacobian matrices that they can determine a value
    // better than the default.
    double column_pivot_threshold = -1;

    // If the Jacobian matrix is near singular, then inverting J'J
    // will result in unreliable results, e.g, if
    //
    //   J = [1.0 1.0         ]
    //       [1.0 1.0000001   ]
    //
    // which is essentially a rank deficient matrix, we have
    //
    //   inv(J'J) = [ 2.0471e+14  -2.0471e+14]
    //              [-2.0471e+14   2.0471e+14]
    //
    // This is not a useful result. Therefore, by default
    // Covariance::Compute will return false if a rank deficient
    // Jacobian is encountered. How rank deficiency is detected
    // depends on the algorithm being used.
    //
    // 1. DENSE_SVD
    //
    //      min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)
    //
    //    where min_sigma and max_sigma are the minimum and maximum
    //    singular values of J respectively.
    //
    // 2. SPARSE_QR
    //
    //      rank(J) < num_col(J)
    //
    //   Here rank(J) is the estimate of the rank of J returned by the
    //   sparse QR factorization algorithm. It is a fairly reliable
    //   indication of rank deficiency.
    //
    double min_reciprocal_condition_number = 1e-14;

    // When using DENSE_SVD, the user has more control in dealing with
    // singular and near singular covariance matrices.
    //
    // As mentioned above, when the covariance matrix is near
    // singular, instead of computing the inverse of J'J, the
    // Moore-Penrose pseudoinverse of J'J should be computed.
    //
    // If J'J has the eigen decomposition (lambda_i, e_i), where
    // lambda_i is the i^th eigenvalue and e_i is the corresponding
    // eigenvector, then the inverse of J'J is
    //
    //   inverse[J'J] = sum_i e_i e_i' / lambda_i
    //
    // and computing the pseudo inverse involves dropping terms from
    // this sum that correspond to small eigenvalues.
    //
    // How terms are dropped is controlled by
    // min_reciprocal_condition_number and null_space_rank.
    //
    // If null_space_rank is non-negative, then the smallest
    // null_space_rank eigenvalue/eigenvectors are dropped
    // irrespective of the magnitude of lambda_i. If the ratio of the
    // smallest non-zero eigenvalue to the largest eigenvalue in the
    // truncated matrix is still below
    // min_reciprocal_condition_number, then the Covariance::Compute()
    // will fail and return false.
    //
    // Setting null_space_rank = -1 drops all terms for which
    //
    //   lambda_i / lambda_max < min_reciprocal_condition_number.
    //
    // This option has no effect on the SUITE_SPARSE_QR and
    // EIGEN_SPARSE_QR algorithms.
    int null_space_rank = 0;

    int num_threads = 1;

    // 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 and in turn its effect on the covariance.
    //
    // TODO(sameergaarwal): Expand this based on Jim's experiments.
    bool apply_loss_function = true;
  };

  explicit Covariance(const Options& options);
  ~Covariance();

  // Compute a part of the covariance matrix.
  //
  // The vector covariance_blocks, indexes into the covariance matrix
  // block-wise using pairs of parameter blocks. This allows the
  // covariance estimation algorithm to only compute and store these
  // blocks.
  //
  // Since the covariance matrix is symmetric, if the user passes
  // (block1, block2), then GetCovarianceBlock can be called with
  // block1, block2 as well as block2, block1.
  //
  // covariance_blocks cannot contain duplicates. Bad things will
  // happen if they do.
  //
  // Note that the list of covariance_blocks is only used to determine
  // what parts of the covariance matrix are computed. The full
  // Jacobian is used to do the computation, i.e. they do not have an
  // impact on what part of the Jacobian is used for computation.
  //
  // The return value indicates the success or failure of the
  // covariance computation. Please see the documentation for
  // Covariance::Options for more on the conditions under which this
  // function returns false.
  bool Compute(const std::vector<std::pair<const double*, const double*>>&
                   covariance_blocks,
               Problem* problem);

  // Compute a part of the covariance matrix.
  //
  // The vector parameter_blocks contains the parameter blocks that
  // are used for computing the covariance matrix. From this vector
  // all covariance pairs are generated. This allows the covariance
  // estimation algorithm to only compute and store these blocks.
  //
  // parameter_blocks cannot contain duplicates. Bad things will
  // happen if they do.
  //
  // Note that the list of covariance_blocks is only used to determine
  // what parts of the covariance matrix are computed. The full
  // Jacobian is used to do the computation, i.e. they do not have an
  // impact on what part of the Jacobian is used for computation.
  //
  // The return value indicates the success or failure of the
  // covariance computation. Please see the documentation for
  // Covariance::Options for more on the conditions under which this
  // function returns false.
  bool Compute(const std::vector<const double*>& parameter_blocks,
               Problem* problem);

  // Return the block of the cross-covariance matrix corresponding to
  // parameter_block1 and parameter_block2.
  //
  // Compute must be called before the first call to
  // GetCovarianceBlock and the pair <parameter_block1,
  // parameter_block2> OR the pair <parameter_block2,
  // parameter_block1> must have been present in the vector
  // covariance_blocks when Compute was called. Otherwise
  // GetCovarianceBlock will return false.
  //
  // covariance_block must point to a memory location that can store a
  // parameter_block1_size x parameter_block2_size matrix. The
  // returned covariance will be a row-major matrix.
  bool GetCovarianceBlock(const double* parameter_block1,
                          const double* parameter_block2,
                          double* covariance_block) const;

  // Returns the block of the cross-covariance in the tangent space if a
  // manifold is associated with either parameter block; else returns
  // cross-covariance in the ambient space.
  //
  // Compute must be called before the first call to
  // GetCovarianceBlock and the pair <parameter_block1,
  // parameter_block2> OR the pair <parameter_block2,
  // parameter_block1> must have been present in the vector
  // covariance_blocks when Compute was called. Otherwise
  // GetCovarianceBlock will return false.
  //
  // covariance_block must point to a memory location that can store a
  // parameter_block1_local_size x parameter_block2_local_size matrix. The
  // returned covariance will be a row-major matrix.
  bool GetCovarianceBlockInTangentSpace(const double* parameter_block1,
                                        const double* parameter_block2,
                                        double* covariance_block) const;

  // Return the covariance matrix corresponding to all parameter_blocks.
  //
  // Compute must be called before calling GetCovarianceMatrix and all
  // parameter_blocks must have been present in the vector
  // parameter_blocks when Compute was called. Otherwise
  // GetCovarianceMatrix returns false.
  //
  // covariance_matrix must point to a memory location that can store
  // the size of the covariance matrix. The covariance matrix will be
  // a square matrix whose row and column count is equal to the sum of
  // the sizes of the individual parameter blocks. The covariance
  // matrix will be a row-major matrix.
  bool GetCovarianceMatrix(const std::vector<const double*>& parameter_blocks,
                           double* covariance_matrix) const;

  // Return the covariance matrix corresponding to parameter_blocks
  // in the tangent space if a manifold is associated with one of the parameter
  // blocks else returns the covariance matrix in the ambient space.
  //
  // Compute must be called before calling GetCovarianceMatrix and all
  // parameter_blocks must have been present in the vector
  // parameters_blocks when Compute was called. Otherwise
  // GetCovarianceMatrix returns false.
  //
  // covariance_matrix must point to a memory location that can store
  // the size of the covariance matrix. The covariance matrix will be
  // a square matrix whose row and column count is equal to the sum of
  // the sizes of the tangent spaces of the individual parameter
  // blocks. The covariance matrix will be a row-major matrix.
  bool GetCovarianceMatrixInTangentSpace(
      const std::vector<const double*>& parameter_blocks,
      double* covariance_matrix) const;

 private:
  std::unique_ptr<internal::CovarianceImpl> impl_;
};

}  // namespace ceres

#include "ceres/internal/reenable_warnings.h"

#endif  // CERES_PUBLIC_COVARIANCE_H_