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@google.com (Sameer Agarwal) 30 31#ifndef CERES_PUBLIC_COVARIANCE_H_ 32#define CERES_PUBLIC_COVARIANCE_H_ 33 34#include <utility> 35#include <vector> 36#include "ceres/internal/port.h" 37#include "ceres/internal/scoped_ptr.h" 38#include "ceres/types.h" 39#include "ceres/internal/disable_warnings.h" 40 41namespace ceres { 42 43class Problem; 44 45namespace internal { 46class CovarianceImpl; 47} // namespace internal 48 49// WARNING 50// ======= 51// It is very easy to use this class incorrectly without understanding 52// the underlying mathematics. Please read and understand the 53// documentation completely before attempting to use this class. 54// 55// 56// This class allows the user to evaluate the covariance for a 57// non-linear least squares problem and provides random access to its 58// blocks 59// 60// Background 61// ========== 62// One way to assess the quality of the solution returned by a 63// non-linear least squares solve is to analyze the covariance of the 64// solution. 65// 66// Let us consider the non-linear regression problem 67// 68// y = f(x) + N(0, I) 69// 70// i.e., the observation y is a random non-linear function of the 71// independent variable x with mean f(x) and identity covariance. Then 72// the maximum likelihood estimate of x given observations y is the 73// solution to the non-linear least squares problem: 74// 75// x* = arg min_x |f(x)|^2 76// 77// And the covariance of x* is given by 78// 79// C(x*) = inverse[J'(x*)J(x*)] 80// 81// Here J(x*) is the Jacobian of f at x*. The above formula assumes 82// that J(x*) has full column rank. 83// 84// If J(x*) is rank deficient, then the covariance matrix C(x*) is 85// also rank deficient and is given by 86// 87// C(x*) = pseudoinverse[J'(x*)J(x*)] 88// 89// Note that in the above, we assumed that the covariance 90// matrix for y was identity. This is an important assumption. If this 91// is not the case and we have 92// 93// y = f(x) + N(0, S) 94// 95// Where S is a positive semi-definite matrix denoting the covariance 96// of y, then the maximum likelihood problem to be solved is 97// 98// x* = arg min_x f'(x) inverse[S] f(x) 99// 100// and the corresponding covariance estimate of x* is given by 101// 102// C(x*) = inverse[J'(x*) inverse[S] J(x*)] 103// 104// So, if it is the case that the observations being fitted to have a 105// covariance matrix not equal to identity, then it is the user's 106// responsibility that the corresponding cost functions are correctly 107// scaled, e.g. in the above case the cost function for this problem 108// should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2} 109// is the inverse square root of the covariance matrix S. 110// 111// This class allows the user to evaluate the covariance for a 112// non-linear least squares problem and provides random access to its 113// blocks. The computation assumes that the CostFunctions compute 114// residuals such that their covariance is identity. 115// 116// Since the computation of the covariance matrix requires computing 117// the inverse of a potentially large matrix, this can involve a 118// rather large amount of time and memory. However, it is usually the 119// case that the user is only interested in a small part of the 120// covariance matrix. Quite often just the block diagonal. This class 121// allows the user to specify the parts of the covariance matrix that 122// she is interested in and then uses this information to only compute 123// and store those parts of the covariance matrix. 124// 125// Rank of the Jacobian 126// -------------------- 127// As we noted above, if the jacobian is rank deficient, then the 128// inverse of J'J is not defined and instead a pseudo inverse needs to 129// be computed. 130// 131// The rank deficiency in J can be structural -- columns which are 132// always known to be zero or numerical -- depending on the exact 133// values in the Jacobian. 134// 135// Structural rank deficiency occurs when the problem contains 136// parameter blocks that are constant. This class correctly handles 137// structural rank deficiency like that. 138// 139// Numerical rank deficiency, where the rank of the matrix cannot be 140// predicted by its sparsity structure and requires looking at its 141// numerical values is more complicated. Here again there are two 142// cases. 143// 144// a. The rank deficiency arises from overparameterization. e.g., a 145// four dimensional quaternion used to parameterize SO(3), which is 146// a three dimensional manifold. In cases like this, the user should 147// use an appropriate LocalParameterization. Not only will this lead 148// to better numerical behaviour of the Solver, it will also expose 149// the rank deficiency to the Covariance object so that it can 150// handle it correctly. 151// 152// b. More general numerical rank deficiency in the Jacobian 153// requires the computation of the so called Singular Value 154// Decomposition (SVD) of J'J. We do not know how to do this for 155// large sparse matrices efficiently. For small and moderate sized 156// problems this is done using dense linear algebra. 157// 158// Gauge Invariance 159// ---------------- 160// In structure from motion (3D reconstruction) problems, the 161// reconstruction is ambiguous upto a similarity transform. This is 162// known as a Gauge Ambiguity. Handling Gauges correctly requires the 163// use of SVD or custom inversion algorithms. For small problems the 164// user can use the dense algorithm. For more details see 165// 166// Ken-ichi Kanatani, Daniel D. Morris: Gauges and gauge 167// transformations for uncertainty description of geometric structure 168// with indeterminacy. IEEE Transactions on Information Theory 47(5): 169// 2017-2028 (2001) 170// 171// Example Usage 172// ============= 173// 174// double x[3]; 175// double y[2]; 176// 177// Problem problem; 178// problem.AddParameterBlock(x, 3); 179// problem.AddParameterBlock(y, 2); 180// <Build Problem> 181// <Solve Problem> 182// 183// Covariance::Options options; 184// Covariance covariance(options); 185// 186// vector<pair<const double*, const double*> > covariance_blocks; 187// covariance_blocks.push_back(make_pair(x, x)); 188// covariance_blocks.push_back(make_pair(y, y)); 189// covariance_blocks.push_back(make_pair(x, y)); 190// 191// CHECK(covariance.Compute(covariance_blocks, &problem)); 192// 193// double covariance_xx[3 * 3]; 194// double covariance_yy[2 * 2]; 195// double covariance_xy[3 * 2]; 196// covariance.GetCovarianceBlock(x, x, covariance_xx) 197// covariance.GetCovarianceBlock(y, y, covariance_yy) 198// covariance.GetCovarianceBlock(x, y, covariance_xy) 199// 200class CERES_EXPORT Covariance { 201 public: 202 struct CERES_EXPORT Options { 203 Options() 204#ifndef CERES_NO_SUITESPARSE 205 : algorithm_type(SUITE_SPARSE_QR), 206#else 207 : algorithm_type(EIGEN_SPARSE_QR), 208#endif 209 min_reciprocal_condition_number(1e-14), 210 null_space_rank(0), 211 num_threads(1), 212 apply_loss_function(true) { 213 } 214 215 // Ceres supports three different algorithms for covariance 216 // estimation, which represent different tradeoffs in speed, 217 // accuracy and reliability. 218 // 219 // 1. DENSE_SVD uses Eigen's JacobiSVD to perform the 220 // computations. It computes the singular value decomposition 221 // 222 // U * S * V' = J 223 // 224 // and then uses it to compute the pseudo inverse of J'J as 225 // 226 // pseudoinverse[J'J]^ = V * pseudoinverse[S] * V' 227 // 228 // It is an accurate but slow method and should only be used 229 // for small to moderate sized problems. It can handle 230 // full-rank as well as rank deficient Jacobians. 231 // 232 // 2. EIGEN_SPARSE_QR uses the sparse QR factorization algorithm 233 // in Eigen to compute the decomposition 234 // 235 // Q * R = J 236 // 237 // [J'J]^-1 = [R*R']^-1 238 // 239 // It is a moderately fast algorithm for sparse matrices. 240 // 241 // 3. SUITE_SPARSE_QR uses the SuiteSparseQR sparse QR 242 // factorization algorithm. It uses dense linear algebra and is 243 // multi threaded, so for large sparse sparse matrices it is 244 // significantly faster than EIGEN_SPARSE_QR. 245 // 246 // Neither EIGEN_SPARSE_QR not SUITE_SPARSE_QR are capable of 247 // computing the covariance if the Jacobian is rank deficient. 248 CovarianceAlgorithmType algorithm_type; 249 250 // If the Jacobian matrix is near singular, then inverting J'J 251 // will result in unreliable results, e.g, if 252 // 253 // J = [1.0 1.0 ] 254 // [1.0 1.0000001 ] 255 // 256 // which is essentially a rank deficient matrix, we have 257 // 258 // inv(J'J) = [ 2.0471e+14 -2.0471e+14] 259 // [-2.0471e+14 2.0471e+14] 260 // 261 // This is not a useful result. Therefore, by default 262 // Covariance::Compute will return false if a rank deficient 263 // Jacobian is encountered. How rank deficiency is detected 264 // depends on the algorithm being used. 265 // 266 // 1. DENSE_SVD 267 // 268 // min_sigma / max_sigma < sqrt(min_reciprocal_condition_number) 269 // 270 // where min_sigma and max_sigma are the minimum and maxiumum 271 // singular values of J respectively. 272 // 273 // 2. SUITE_SPARSE_QR and EIGEN_SPARSE_QR 274 // 275 // rank(J) < num_col(J) 276 // 277 // Here rank(J) is the estimate of the rank of J returned by the 278 // sparse QR factorization algorithm. It is a fairly reliable 279 // indication of rank deficiency. 280 // 281 double min_reciprocal_condition_number; 282 283 // When using DENSE_SVD, the user has more control in dealing with 284 // singular and near singular covariance matrices. 285 // 286 // As mentioned above, when the covariance matrix is near 287 // singular, instead of computing the inverse of J'J, the 288 // Moore-Penrose pseudoinverse of J'J should be computed. 289 // 290 // If J'J has the eigen decomposition (lambda_i, e_i), where 291 // lambda_i is the i^th eigenvalue and e_i is the corresponding 292 // eigenvector, then the inverse of J'J is 293 // 294 // inverse[J'J] = sum_i e_i e_i' / lambda_i 295 // 296 // and computing the pseudo inverse involves dropping terms from 297 // this sum that correspond to small eigenvalues. 298 // 299 // How terms are dropped is controlled by 300 // min_reciprocal_condition_number and null_space_rank. 301 // 302 // If null_space_rank is non-negative, then the smallest 303 // null_space_rank eigenvalue/eigenvectors are dropped 304 // irrespective of the magnitude of lambda_i. If the ratio of the 305 // smallest non-zero eigenvalue to the largest eigenvalue in the 306 // truncated matrix is still below 307 // min_reciprocal_condition_number, then the Covariance::Compute() 308 // will fail and return false. 309 // 310 // Setting null_space_rank = -1 drops all terms for which 311 // 312 // lambda_i / lambda_max < min_reciprocal_condition_number. 313 // 314 // This option has no effect on the SUITE_SPARSE_QR and 315 // EIGEN_SPARSE_QR algorithms. 316 int null_space_rank; 317 318 int num_threads; 319 320 // Even though the residual blocks in the problem may contain loss 321 // functions, setting apply_loss_function to false will turn off 322 // the application of the loss function to the output of the cost 323 // function and in turn its effect on the covariance. 324 // 325 // TODO(sameergaarwal): Expand this based on Jim's experiments. 326 bool apply_loss_function; 327 }; 328 329 explicit Covariance(const Options& options); 330 ~Covariance(); 331 332 // Compute a part of the covariance matrix. 333 // 334 // The vector covariance_blocks, indexes into the covariance matrix 335 // block-wise using pairs of parameter blocks. This allows the 336 // covariance estimation algorithm to only compute and store these 337 // blocks. 338 // 339 // Since the covariance matrix is symmetric, if the user passes 340 // (block1, block2), then GetCovarianceBlock can be called with 341 // block1, block2 as well as block2, block1. 342 // 343 // covariance_blocks cannot contain duplicates. Bad things will 344 // happen if they do. 345 // 346 // Note that the list of covariance_blocks is only used to determine 347 // what parts of the covariance matrix are computed. The full 348 // Jacobian is used to do the computation, i.e. they do not have an 349 // impact on what part of the Jacobian is used for computation. 350 // 351 // The return value indicates the success or failure of the 352 // covariance computation. Please see the documentation for 353 // Covariance::Options for more on the conditions under which this 354 // function returns false. 355 bool Compute( 356 const vector<pair<const double*, const double*> >& covariance_blocks, 357 Problem* problem); 358 359 // Return the block of the covariance matrix corresponding to 360 // parameter_block1 and parameter_block2. 361 // 362 // Compute must be called before the first call to 363 // GetCovarianceBlock and the pair <parameter_block1, 364 // parameter_block2> OR the pair <parameter_block2, 365 // parameter_block1> must have been present in the vector 366 // covariance_blocks when Compute was called. Otherwise 367 // GetCovarianceBlock will return false. 368 // 369 // covariance_block must point to a memory location that can store a 370 // parameter_block1_size x parameter_block2_size matrix. The 371 // returned covariance will be a row-major matrix. 372 bool GetCovarianceBlock(const double* parameter_block1, 373 const double* parameter_block2, 374 double* covariance_block) const; 375 376 private: 377 internal::scoped_ptr<internal::CovarianceImpl> impl_; 378}; 379 380} // namespace ceres 381 382#include "ceres/internal/reenable_warnings.h" 383 384#endif // CERES_PUBLIC_COVARIANCE_H_ 385