forked from bartvdbraak/blender
72 lines
2.7 KiB
Diff
72 lines
2.7 KiB
Diff
|
diff --git a/src/libmv/numeric/levenberg_marquardt.h b/src/libmv/numeric/levenberg_marquardt.h
|
||
|
index 6a54f66..4473b72 100644
|
||
|
--- a/src/libmv/numeric/levenberg_marquardt.h
|
||
|
+++ b/src/libmv/numeric/levenberg_marquardt.h
|
||
|
@@ -33,6 +33,7 @@
|
||
|
|
||
|
#include "libmv/numeric/numeric.h"
|
||
|
#include "libmv/numeric/function_derivative.h"
|
||
|
+#include "libmv/logging/logging.h"
|
||
|
|
||
|
namespace libmv {
|
||
|
|
||
|
@@ -123,26 +124,40 @@ class LevenbergMarquardt {
|
||
|
Parameters dx, x_new;
|
||
|
int i;
|
||
|
for (i = 0; results.status == RUNNING && i < params.max_iterations; ++i) {
|
||
|
- if (dx.norm() <= params.relative_step_threshold * x.norm()) {
|
||
|
+ VLOG(1) << "iteration: " << i;
|
||
|
+ VLOG(1) << "||f(x)||: " << f_(x).norm();
|
||
|
+ VLOG(1) << "max(g): " << g.array().abs().maxCoeff();
|
||
|
+ VLOG(1) << "u: " << u;
|
||
|
+ VLOG(1) << "v: " << v;
|
||
|
+
|
||
|
+ AMatrixType A_augmented = A + u*AMatrixType::Identity(J.cols(), J.cols());
|
||
|
+ Solver solver(A_augmented);
|
||
|
+ dx = solver.solve(g);
|
||
|
+ bool solved = (A_augmented * dx).isApprox(g);
|
||
|
+ if (!solved) {
|
||
|
+ LOG(ERROR) << "Failed to solve";
|
||
|
+ }
|
||
|
+ if (solved && dx.norm() <= params.relative_step_threshold * x.norm()) {
|
||
|
results.status = RELATIVE_STEP_SIZE_TOO_SMALL;
|
||
|
break;
|
||
|
- }
|
||
|
- x_new = x + dx;
|
||
|
- // Rho is the ratio of the actual reduction in error to the reduction
|
||
|
- // in error that would be obtained if the problem was linear.
|
||
|
- // See [1] for details.
|
||
|
- Scalar rho((error.squaredNorm() - f_(x_new).squaredNorm())
|
||
|
- / dx.dot(u*dx + g));
|
||
|
- if (rho > 0) {
|
||
|
- // Accept the Gauss-Newton step because the linear model fits well.
|
||
|
- x = x_new;
|
||
|
- results.status = Update(x, params, &J, &A, &error, &g);
|
||
|
- Scalar tmp = Scalar(2*rho-1);
|
||
|
- u = u*std::max(1/3., 1 - (tmp*tmp*tmp));
|
||
|
- v = 2;
|
||
|
- continue;
|
||
|
- }
|
||
|
-
|
||
|
+ }
|
||
|
+ if (solved) {
|
||
|
+ x_new = x + dx;
|
||
|
+ // Rho is the ratio of the actual reduction in error to the reduction
|
||
|
+ // in error that would be obtained if the problem was linear.
|
||
|
+ // See [1] for details.
|
||
|
+ Scalar rho((error.squaredNorm() - f_(x_new).squaredNorm())
|
||
|
+ / dx.dot(u*dx + g));
|
||
|
+ if (rho > 0) {
|
||
|
+ // Accept the Gauss-Newton step because the linear model fits well.
|
||
|
+ x = x_new;
|
||
|
+ results.status = Update(x, params, &J, &A, &error, &g);
|
||
|
+ Scalar tmp = Scalar(2*rho-1);
|
||
|
+ u = u*std::max(1/3., 1 - (tmp*tmp*tmp));
|
||
|
+ v = 2;
|
||
|
+ continue;
|
||
|
+ }
|
||
|
+ }
|
||
|
// Reject the update because either the normal equations failed to solve
|
||
|
// or the local linear model was not good (rho < 0). Instead, increase u
|
||
|
// to move closer to gradient descent.
|