29 #ifndef LIBMV_NUMERIC_DOGLEG_H
30 #define LIBMV_NUMERIC_DOGLEG_H
41 template <
typename Function,
42 typename Jacobian = NumericJacobian<Function>,
43 typename Solver = Eigen::PartialPivLU<
44 Matrix<
typename Function::FMatrixType::RealScalar,
45 Function::XMatrixType::RowsAtCompileTime,
46 Function::XMatrixType::RowsAtCompileTime>>>
49 typedef typename Function::XMatrixType::RealScalar
Scalar;
50 typedef typename Function::FMatrixType
FVec;
52 typedef Matrix<
typename Function::FMatrixType::RealScalar,
53 Function::FMatrixType::RowsAtCompileTime,
54 Function::XMatrixType::RowsAtCompileTime>
56 typedef Matrix<
typename JMatrixType::RealScalar,
57 JMatrixType::ColsAtCompileTime,
58 JMatrixType::ColsAtCompileTime>
76 Dogleg(
const Function& f) : f_(f), df_(f) {}
108 *
A = (*J).transpose() * (*J);
110 *
g = (*J).transpose() * *
error;
111 if (
g->array().abs().maxCoeff() <
params.gradient_threshold) {
113 }
else if (
error->array().abs().maxCoeff() <
params.error_threshold) {
127 if (dx_gn.norm() < radius) {
131 }
else if (alpha * dx_sd.norm() > radius) {
132 *dx_dl = (radius / dx_sd.norm()) * dx_sd;
139 Scalar Mbma2 = b_minus_a.squaredNorm();
142 Scalar radius2 = radius * radius;
144 *
beta = (-
c +
sqrt(
c *
c + Mbma2 * (radius2 - Ma2))) / (Mbma2);
146 *
beta = (radius2 - Ma2) / (
c +
sqrt(
c *
c + Mbma2 * (radius2 - Ma2)));
148 *dx_dl = alpha * dx_sd + (*beta) * (dx_gn - alpha * dx_sd);
169 bool x_updated =
true;
175 printf(
"iteration ||f(x)|| max(g) radius\n");
178 printf(
"%9d %12g %12g %12g",
181 g.array().abs().maxCoeff(),
189 Scalar alpha =
g.squaredNorm() / (J *
g).squaredNorm();
200 dx_gn = solver.solve(-
g);
201 if (!(
A * dx_gn).isApprox(-
g)) {
202 LOG(ERROR) <<
"Failed to solve normal eqns. TODO: Solve via SVD.";
214 if (dx_dl.norm() < e3 * (
x.norm() + e3)) {
220 Scalar actual = f_(
x).squaredNorm() - f_(x_new).squaredNorm();
223 predicted = f_(
x).squaredNorm();
225 predicted = radius * (2 * alpha *
g.norm() - radius) / 2 / alpha;
226 }
else if (step ==
DOGLEG) {
227 predicted = 0.5 * alpha * (1 -
beta) * (1 -
beta) *
g.squaredNorm() +
230 Scalar rho = actual / predicted;
239 printf(
" %12g %12g %12g\n", rho, actual, predicted);
248 radius =
std::max(radius, 3 * dx_dl.norm());
249 }
else if (rho < 0.25) {
251 if (radius < e3 * (
x.norm() + e3)) {
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Matrix< typename Function::FMatrixType::RealScalar, Function::FMatrixType::RowsAtCompileTime, Function::XMatrixType::RowsAtCompileTime > JMatrixType
Results minimize(const SolverParameters ¶ms, Parameters *x_and_min)
Status Update(const Parameters &x, const SolverParameters ¶ms, JMatrixType *J, AMatrixType *A, FVec *error, Parameters *g)
Function::XMatrixType::RealScalar Scalar
Function::FMatrixType FVec
@ RELATIVE_STEP_SIZE_TOO_SMALL
Matrix< typename JMatrixType::RealScalar, JMatrixType::ColsAtCompileTime, JMatrixType::ColsAtCompileTime > AMatrixType
Function::XMatrixType Parameters
Dogleg(const Function &f)
Results minimize(Parameters *x_and_min)
Step SolveDoglegDirection(const Parameters &dx_sd, const Parameters &dx_gn, Scalar radius, Scalar alpha, Parameters *dx_dl, Scalar *beta)
static void error(const char *str)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
static const pxr::TfToken g("g", pxr::TfToken::Immortal)
Scalar gradient_magnitude
Scalar relative_step_threshold
Scalar initial_trust_radius
Scalar gradient_threshold
ccl_device_inline float beta(float x, float y)