1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2010, 2011, 2012 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 #include "ceres/corrector.h"
32
33 #include <cstddef>
34 #include <cmath>
35 #include "ceres/internal/eigen.h"
36 #include "glog/logging.h"
37
38 namespace ceres {
39 namespace internal {
40
Corrector(double sq_norm,const double rho[3])41 Corrector::Corrector(double sq_norm, const double rho[3]) {
42 CHECK_GE(sq_norm, 0.0);
43 CHECK_GT(rho[1], 0.0);
44 sqrt_rho1_ = sqrt(rho[1]);
45
46 // If sq_norm = 0.0, the correction becomes trivial, the residual
47 // and the jacobian are scaled by the squareroot of the derivative
48 // of rho. Handling this case explicitly avoids the divide by zero
49 // error that would occur below.
50 //
51 // The case where rho'' < 0 also gets special handling. Technically
52 // it shouldn't, and the computation of the scaling should proceed
53 // as below, however we found in experiments that applying the
54 // curvature correction when rho'' < 0, which is the case when we
55 // are in the outlier region slows down the convergence of the
56 // algorithm significantly.
57 //
58 // Thus, we have divided the action of the robustifier into two
59 // parts. In the inliner region, we do the full second order
60 // correction which re-wights the gradient of the function by the
61 // square root of the derivative of rho, and the Gauss-Newton
62 // Hessian gets both the scaling and the rank-1 curvature
63 // correction. Normaly, alpha is upper bounded by one, but with this
64 // change, alpha is bounded above by zero.
65 //
66 // Empirically we have observed that the full Triggs correction and
67 // the clamped correction both start out as very good approximations
68 // to the loss function when we are in the convex part of the
69 // function, but as the function starts transitioning from convex to
70 // concave, the Triggs approximation diverges more and more and
71 // ultimately becomes linear. The clamped Triggs model however
72 // remains quadratic.
73 //
74 // The reason why the Triggs approximation becomes so poor is
75 // because the curvature correction that it applies to the gauss
76 // newton hessian goes from being a full rank correction to a rank
77 // deficient correction making the inversion of the Hessian fraught
78 // with all sorts of misery and suffering.
79 //
80 // The clamped correction retains its quadratic nature and inverting it
81 // is always well formed.
82 if ((sq_norm == 0.0) || (rho[2] <= 0.0)) {
83 residual_scaling_ = sqrt_rho1_;
84 alpha_sq_norm_ = 0.0;
85 return;
86 }
87
88 // Calculate the smaller of the two solutions to the equation
89 //
90 // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0.
91 //
92 // Start by calculating the discriminant D.
93 const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1];
94
95 // Since both rho[1] and rho[2] are guaranteed to be positive at
96 // this point, we know that D > 1.0.
97
98 const double alpha = 1.0 - sqrt(D);
99
100 // Calculate the constants needed by the correction routines.
101 residual_scaling_ = sqrt_rho1_ / (1 - alpha);
102 alpha_sq_norm_ = alpha / sq_norm;
103 }
104
CorrectResiduals(int nrow,double * residuals)105 void Corrector::CorrectResiduals(int nrow, double* residuals) {
106 DCHECK(residuals != NULL);
107 VectorRef r_ref(residuals, nrow);
108 // Equation 11 in BANS.
109 r_ref *= residual_scaling_;
110 }
111
CorrectJacobian(int nrow,int ncol,double * residuals,double * jacobian)112 void Corrector::CorrectJacobian(int nrow, int ncol,
113 double* residuals, double* jacobian) {
114 DCHECK(residuals != NULL);
115 DCHECK(jacobian != NULL);
116 ConstVectorRef r_ref(residuals, nrow);
117 MatrixRef j_ref(jacobian, nrow, ncol);
118
119 // Equation 11 in BANS.
120 j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ *
121 r_ref * (r_ref.transpose() * j_ref));
122 }
123
124 } // namespace internal
125 } // namespace ceres
126