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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
|
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2015 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_INTERNAL_DOGLEG_STRATEGY_H_
#define CERES_INTERNAL_DOGLEG_STRATEGY_H_
#include "ceres/internal/port.h"
#include "ceres/linear_solver.h"
#include "ceres/trust_region_strategy.h"
namespace ceres {
namespace internal {
// Dogleg step computation and trust region sizing strategy based on
// on "Methods for Nonlinear Least Squares" by K. Madsen, H.B. Nielsen
// and O. Tingleff. Available to download from
//
// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
//
// One minor modification is that instead of computing the pure
// Gauss-Newton step, we compute a regularized version of it. This is
// because the Jacobian is often rank-deficient and in such cases
// using a direct solver leads to numerical failure.
//
// If SUBSPACE is passed as the type argument to the constructor, the
// DoglegStrategy follows the approach by Shultz, Schnabel, Byrd.
// This finds the exact optimum over the two-dimensional subspace
// spanned by the two Dogleg vectors.
class CERES_EXPORT_INTERNAL DoglegStrategy : public TrustRegionStrategy {
public:
explicit DoglegStrategy(const TrustRegionStrategy::Options& options);
virtual ~DoglegStrategy() {}
// TrustRegionStrategy interface
Summary ComputeStep(const PerSolveOptions& per_solve_options,
SparseMatrix* jacobian,
const double* residuals,
double* step) final;
void StepAccepted(double step_quality) final;
void StepRejected(double step_quality) final;
void StepIsInvalid();
double Radius() const final;
// These functions are predominantly for testing.
Vector gradient() const { return gradient_; }
Vector gauss_newton_step() const { return gauss_newton_step_; }
Matrix subspace_basis() const { return subspace_basis_; }
Vector subspace_g() const { return subspace_g_; }
Matrix subspace_B() const { return subspace_B_; }
private:
typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Vector2d;
typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Matrix2d;
LinearSolver::Summary ComputeGaussNewtonStep(
const PerSolveOptions& per_solve_options,
SparseMatrix* jacobian,
const double* residuals);
void ComputeCauchyPoint(SparseMatrix* jacobian);
void ComputeGradient(SparseMatrix* jacobian, const double* residuals);
void ComputeTraditionalDoglegStep(double* step);
bool ComputeSubspaceModel(SparseMatrix* jacobian);
void ComputeSubspaceDoglegStep(double* step);
bool FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const;
Vector MakePolynomialForBoundaryConstrainedProblem() const;
Vector2d ComputeSubspaceStepFromRoot(double lambda) const;
double EvaluateSubspaceModel(const Vector2d& x) const;
LinearSolver* linear_solver_;
double radius_;
const double max_radius_;
const double min_diagonal_;
const double max_diagonal_;
// mu is used to scale the diagonal matrix used to make the
// Gauss-Newton solve full rank. In each solve, the strategy starts
// out with mu = min_mu, and tries values up to max_mu. If the user
// reports an invalid step, the value of mu_ is increased so that
// the next solve starts with a stronger regularization.
//
// If a successful step is reported, then the value of mu_ is
// decreased with a lower bound of min_mu_.
double mu_;
const double min_mu_;
const double max_mu_;
const double mu_increase_factor_;
const double increase_threshold_;
const double decrease_threshold_;
Vector diagonal_; // sqrt(diag(J^T J))
Vector lm_diagonal_;
Vector gradient_;
Vector gauss_newton_step_;
// cauchy_step = alpha * gradient
double alpha_;
double dogleg_step_norm_;
// When, ComputeStep is called, reuse_ indicates whether the
// Gauss-Newton and Cauchy steps from the last call to ComputeStep
// can be reused or not.
//
// If the user called StepAccepted, then it is expected that the
// user has recomputed the Jacobian matrix and new Gauss-Newton
// solve is needed and reuse is set to false.
//
// If the user called StepRejected, then it is expected that the
// user wants to solve the trust region problem with the same matrix
// but a different trust region radius and the Gauss-Newton and
// Cauchy steps can be reused to compute the Dogleg, thus reuse is
// set to true.
//
// If the user called StepIsInvalid, then there was a numerical
// problem with the step computed in the last call to ComputeStep,
// and the regularization used to do the Gauss-Newton solve is
// increased and a new solve should be done when ComputeStep is
// called again, thus reuse is set to false.
bool reuse_;
// The dogleg type determines how the minimum of the local
// quadratic model is found.
DoglegType dogleg_type_;
// If the type is SUBSPACE_DOGLEG, the two-dimensional
// model 1/2 x^T B x + g^T x has to be computed and stored.
bool subspace_is_one_dimensional_;
Matrix subspace_basis_;
Vector2d subspace_g_;
Matrix2d subspace_B_;
};
} // namespace internal
} // namespace ceres
#endif // CERES_INTERNAL_DOGLEG_STRATEGY_H_
|
