10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A preconditioned conjugate gradients solver 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// (ConjugateGradientsSolver) for positive semidefinite linear 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// systems. 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// We have also augmented the termination criterion used by this 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// solver to support not just residual based termination but also 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// termination based on decrease in the value of the quadratic model 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// that CG optimizes. 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/conjugate_gradients_solver.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cmath> 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstddef> 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/fpclassify.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_operator.h" 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace { 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool IsZeroOrInfinity(double x) { 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return ((x == 0.0) || (IsInfinite(x))); 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Constant used in the MATLAB implementation ~ 2 * eps. 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kEpsilon = 2.2204e-16; 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongConjugateGradientsSolver::ConjugateGradientsSolver( 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LinearSolver::Options& options) 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : options_(options) { 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongLinearSolver::Summary ConjugateGradientsSolver::Solve( 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearOperator* A, 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* b, 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LinearSolver::PerSolveOptions& per_solve_options, 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x) { 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(A); 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(b); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(A->num_rows(), A->num_cols()); 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearSolver::Summary summary; 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = MAX_ITERATIONS; 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.num_iterations = 0; 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols = A->num_cols(); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef xref(x, num_cols); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ConstVectorRef bref(b, num_cols); 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm_b = bref.norm(); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (norm_b == 0.0) { 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong xref.setZero(); 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = TOLERANCE; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return summary; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector r(num_cols); 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector p(num_cols); 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector z(num_cols); 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector tmp(num_cols); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double tol_r = per_solve_options.r_tolerance * norm_b; 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp.setZero(); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->RightMultiply(x, tmp.data()); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong r = bref - tmp; 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double norm_r = r.norm(); 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (norm_r <= tol_r) { 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = TOLERANCE; 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return summary; 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double rho = 1.0; 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Initial value of the quadratic model Q = x'Ax - 2 * b'x. 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double Q0 = -1.0 * xref.dot(bref + r); 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (summary.num_iterations = 1; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.num_iterations < options_.max_num_iterations; 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++summary.num_iterations) { 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(3) << "cg iteration " << summary.num_iterations; 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Apply preconditioner 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (per_solve_options.preconditioner != NULL) { 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong z.setZero(); 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong per_solve_options.preconditioner->RightMultiply(r.data(), z.data()); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong z = r; 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double last_rho = rho; 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rho = r.dot(z); 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsZeroOrInfinity(rho)) { 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "Numerical failure. rho = " << rho; 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = FAILURE; 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (summary.num_iterations == 1) { 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong p = z; 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double beta = rho / last_rho; 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsZeroOrInfinity(beta)) { 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "Numerical failure. beta = " << beta; 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = FAILURE; 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong p = z + beta * p; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector& q = z; 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong q.setZero(); 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->RightMultiply(p.data(), q.data()); 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double pq = p.dot(q); 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((pq <= 0) || IsInfinite(pq)) { 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "Numerical failure. pq = " << pq; 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = FAILURE; 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double alpha = rho / pq; 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsInfinite(alpha)) { 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(ERROR) << "Numerical failure. alpha " << alpha; 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = FAILURE; 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong xref = xref + alpha * p; 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Ideally we would just use the update r = r - alpha*q to keep 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // track of the residual vector. However this estimate tends to 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // drift over time due to round off errors. Thus every 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // residual_reset_period iterations, we calculate the residual as 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // r = b - Ax. We do not do this every iteration because this 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // requires an additional matrix vector multiply which would 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // double the complexity of the CG algorithm. 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (summary.num_iterations % options_.residual_reset_period == 0) { 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp.setZero(); 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A->RightMultiply(x, tmp.data()); 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong r = bref - tmp; 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong r = r - alpha * q; 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Quadratic model based termination. 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Q1 = x'Ax - 2 * b' x. 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double Q1 = -1.0 * xref.dot(bref + r); 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For PSD matrices A, let 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Q(x) = x'Ax - 2b'x 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // be the cost of the quadratic function defined by A and b. Then, 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the solver terminates at iteration i if 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This termination criterion is more useful when using CG to 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // solve the Newton step. This particular convergence test comes 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // from Stephen Nash's work on truncated Newton 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // methods. References: 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Direction Within A Truncated Newton Method, Operation 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Research Letters 9(1990) 219-221. 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Journal of Computational and Applied Mathematics, 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 124(1-2), 45-59, 2000. 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double zeta = summary.num_iterations * (Q1 - Q0) / Q1; 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(3) << "Q termination: zeta " << zeta 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << per_solve_options.q_tolerance; 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (zeta < per_solve_options.q_tolerance) { 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = TOLERANCE; 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Q0 = Q1; 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Residual based termination. 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong norm_r = r. norm(); 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(3) << "R termination: norm_r " << norm_r 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << tol_r; 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (norm_r <= tol_r) { 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong summary.termination_type = TOLERANCE; 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return summary; 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 234