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