Fix the jacobian scaling bug.
If the norm of a column in the jacobian is near zero.
The jacobian scaling could in trying to avoid division
by zero actually make things much worse. It made it
appear that the column actually had mass when in fact
it did not.
This leads to inflated values for the parameters for
that column when we get some numerical garbage back
from the linear solver.
This should also address the case where users are setting
some columns to zero to hold some parameter constant.
The fix is a bit delicate, and frankly I am not
completely sure that it fixes the issue. But right now
I am quite certain that the current implementation is
better than what we had earlier. Whether this is the
best fix is not entirely certain.
To test the fix, I converted Arnaud Gelas' original
code which triggered this bug into a test case.
Change-Id: Idbbd8177269bdc06338e0f54410f93ddc127c8ca
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
index 76c4f8a..194d54e 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -77,7 +77,7 @@
double* scale) const {
jacobian.SquaredColumnNorm(scale);
for (int i = 0; i < jacobian.num_cols(); ++i) {
- scale[i] = 1.0 / (kEpsilon + sqrt(scale[i]));
+ scale[i] = 1.0 / (1.0 + sqrt(scale[i]));
}
}
diff --git a/internal/ceres/trust_region_minimizer_test.cc b/internal/ceres/trust_region_minimizer_test.cc
index 414567a..9d5af1a 100644
--- a/internal/ceres/trust_region_minimizer_test.cc
+++ b/internal/ceres/trust_region_minimizer_test.cc
@@ -34,12 +34,14 @@
// Program and Problem machinery.
#include <cmath>
+#include "ceres/cost_function.h"
#include "ceres/dense_qr_solver.h"
#include "ceres/dense_sparse_matrix.h"
#include "ceres/evaluator.h"
#include "ceres/internal/port.h"
#include "ceres/linear_solver.h"
#include "ceres/minimizer.h"
+#include "ceres/problem.h"
#include "ceres/trust_region_minimizer.h"
#include "ceres/trust_region_strategy.h"
#include "gtest/gtest.h"
@@ -277,5 +279,94 @@
IsTrustRegionSolveSuccessful<false, false, false, true >(kStrategy);
}
+
+class CurveCostFunction : public CostFunction {
+ public:
+ CurveCostFunction(int num_vertices, double target_length)
+ : num_vertices_(num_vertices), target_length_(target_length) {
+ set_num_residuals(1);
+ for (int i = 0; i < num_vertices_; ++i) {
+ mutable_parameter_block_sizes()->push_back(2);
+ }
+ }
+
+ bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ residuals[0] = target_length_;
+
+ for (int i = 0; i < num_vertices_; ++i) {
+ int prev = (num_vertices_ + i - 1) % num_vertices_;
+ double length = 0.0;
+ for (int dim = 0; dim < 2; dim++) {
+ const double diff = parameters[prev][dim] - parameters[i][dim];
+ length += diff * diff;
+ }
+ residuals[0] -= sqrt(length);
+ }
+
+ if (jacobians == NULL) {
+ return true;
+ }
+
+ for (int i = 0; i < num_vertices_; ++i) {
+ if (jacobians[i] != NULL) {
+ int prev = (num_vertices_ + i - 1) % num_vertices_;
+ int next = (i + 1) % num_vertices_;
+
+ double u[2], v[2];
+ double norm_u = 0., norm_v = 0.;
+ for (int dim = 0; dim < 2; dim++) {
+ u[dim] = parameters[i][dim] - parameters[prev][dim];
+ norm_u += u[dim] * u[dim];
+ v[dim] = parameters[next][dim] - parameters[i][dim];
+ norm_v += v[dim] * v[dim];
+ }
+
+ norm_u = sqrt(norm_u);
+ norm_v = sqrt(norm_v);
+
+ for (int dim = 0; dim < 2; dim++) {
+ jacobians[i][dim] = 0.;
+
+ if (norm_u > std::numeric_limits< double >::min()) {
+ jacobians[i][dim] -= u[dim] / norm_u;
+ }
+
+ if (norm_v > std::numeric_limits< double >::min()) {
+ jacobians[i][dim] += v[dim] / norm_v;
+ }
+ }
+ }
+ }
+
+ return true;
+ }
+
+ private:
+ int num_vertices_;
+ double target_length_;
+};
+
+TEST(TrustRegionMinimizer, JacobiScalingTest) {
+ int N = 6;
+ std::vector< double* > y(N);
+ const double pi = 3.1415926535897932384626433;
+ for (int i = 0; i < N; i++) {
+ double theta = i * 2. * pi/ static_cast< double >(N);
+ y[i] = new double[2];
+ y[i][0] = cos(theta);
+ y[i][1] = sin(theta);
+ }
+
+ Problem problem;
+ problem.AddResidualBlock(new CurveCostFunction(N, 10.), NULL, y);
+ Solver::Options options;
+ options.linear_solver_type = ceres::DENSE_QR;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_LE(summary.final_cost, 1e-10);
+}
+
} // namespace internal
} // namespace ceres
