Add LocalParameterization::MultiplyByJacobian. This is needed to efficiently support LocalParameterization objects in GradientProblemSolver. Change-Id: Ic7b715b8be694b099dc95d6707a67474297533e6
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h index 3ab21fc..656c4d4 100644 --- a/include/ceres/local_parameterization.h +++ b/include/ceres/local_parameterization.h
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -110,7 +110,7 @@ // Jacobian which is needed to compute the Jacobian of f w.r.t delta. class CERES_EXPORT LocalParameterization { public: - virtual ~LocalParameterization() {} + virtual ~LocalParameterization(); // Generalization of the addition operation, // @@ -122,8 +122,23 @@ double* x_plus_delta) const = 0; // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. + // + // jacobian is a row-major GlobalSize() x LocalSize() matrix. virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; + // local_matrix = global_matrix * jacobian + // + // global_matrix is a num_rows x GlobalSize row major matrix. + // local_matrix is a num_rows x LocalSize row major matrix. + // jacobian(x) is the matrix returned by ComputeJacobian at x. + // + // This is only used by GradientProblem. For most normal uses, it is + // okay to use the default implementation. + virtual bool MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const; + // Size of x. virtual int GlobalSize() const = 0; @@ -143,6 +158,10 @@ double* x_plus_delta) const; virtual bool ComputeJacobian(const double* x, double* jacobian) const; + virtual bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const; virtual int GlobalSize() const { return size_; } virtual int LocalSize() const { return size_; } @@ -161,6 +180,10 @@ double* x_plus_delta) const; virtual bool ComputeJacobian(const double* x, double* jacobian) const; + virtual bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const; virtual int GlobalSize() const { return static_cast<int>(constancy_mask_.size()); }
diff --git a/internal/ceres/autodiff_local_parameterization_test.cc b/internal/ceres/autodiff_local_parameterization_test.cc index a0f705d..a73c6a6 100644 --- a/internal/ceres/autodiff_local_parameterization_test.cc +++ b/internal/ceres/autodiff_local_parameterization_test.cc
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2013 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without
diff --git a/internal/ceres/local_parameterization.cc b/internal/ceres/local_parameterization.cc index 26e7f49..a4832c5 100644 --- a/internal/ceres/local_parameterization.cc +++ b/internal/ceres/local_parameterization.cc
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -36,6 +36,23 @@ namespace ceres { +LocalParameterization::~LocalParameterization() { +} + +bool LocalParameterization::MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const { + Matrix jacobian(GlobalSize(), LocalSize()); + if (!ComputeJacobian(x, jacobian.data())) { + return false; + } + + MatrixRef(local_matrix, num_rows, LocalSize()) = + ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; + return true; +} + IdentityParameterization::IdentityParameterization(const int size) : size_(size) { CHECK_GT(size, 0); @@ -55,6 +72,16 @@ return true; } +bool IdentityParameterization::MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const { + std::copy(global_matrix, + global_matrix + num_cols * GlobalSize(), + local_matrix); + return true; +} + SubsetParameterization::SubsetParameterization( int size, const vector<int>& constant_parameters) @@ -108,6 +135,21 @@ return true; } +bool SubsetParameterization::MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const { + for (int row = 0; row < num_rows; ++row) { + for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) { + if (!constancy_mask_[col]) { + local_matrix[row * LocalSize() + j++] = + global_matrix[row * GlobalSize() + col]; + } + } + } + return true; +} + bool QuaternionParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const {
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc index 9b775b4..6e7e87b 100644 --- a/internal/ceres/local_parameterization_test.cc +++ b/internal/ceres/local_parameterization_test.cc
@@ -60,6 +60,14 @@ EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0); } } + + Matrix global_matrix = Matrix::Ones(10, 3); + Matrix local_matrix = Matrix::Zero(10, 3); + parameterization.MultiplyByJacobian(x, + 10, + global_matrix.data(), + local_matrix.data()); + EXPECT_EQ((local_matrix - global_matrix).norm(), 0.0); } TEST(SubsetParameterization, DeathTests) { @@ -85,17 +93,20 @@ } TEST(SubsetParameterization, NormalFunctionTest) { - double x[4] = {1.0, 2.0, 3.0, 4.0}; - for (int i = 0; i < 4; ++i) { + const int kGlobalSize = 4; + const int kLocalSize = 3; + + double x[kGlobalSize] = {1.0, 2.0, 3.0, 4.0}; + for (int i = 0; i < kGlobalSize; ++i) { vector<int> constant_parameters; constant_parameters.push_back(i); - SubsetParameterization parameterization(4, constant_parameters); - double delta[3] = {1.0, 2.0, 3.0}; - double x_plus_delta[4] = {0.0, 0.0, 0.0}; + SubsetParameterization parameterization(kGlobalSize, constant_parameters); + double delta[kLocalSize] = {1.0, 2.0, 3.0}; + double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0}; parameterization.Plus(x, delta, x_plus_delta); int k = 0; - for (int j = 0; j < 4; ++j) { + for (int j = 0; j < kGlobalSize; ++j) { if (j == i) { EXPECT_EQ(x_plus_delta[j], x[j]); } else { @@ -103,22 +114,38 @@ } } - double jacobian[4 * 3]; + double jacobian[kGlobalSize * kLocalSize]; parameterization.ComputeJacobian(x, jacobian); int delta_cursor = 0; int jacobian_cursor = 0; - for (int j = 0; j < 4; ++j) { + for (int j = 0; j < kGlobalSize; ++j) { if (j != i) { - for (int k = 0; k < 3; ++k, jacobian_cursor++) { + for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) { EXPECT_EQ(jacobian[jacobian_cursor], delta_cursor == k ? 1.0 : 0.0); } ++delta_cursor; } else { - for (int k = 0; k < 3; ++k, jacobian_cursor++) { + for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) { EXPECT_EQ(jacobian[jacobian_cursor], 0.0); } } } + + Matrix global_matrix = Matrix::Ones(10, kGlobalSize); + for (int row = 0; row < kGlobalSize; ++row) { + for (int col = 0; col < kGlobalSize; ++col) { + global_matrix(row, col) = col; + } + } + + Matrix local_matrix = Matrix::Zero(10, kLocalSize); + parameterization.MultiplyByJacobian(x, + 10, + global_matrix.data(), + local_matrix.data()); + Matrix expected_local_matrix = + global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize); + EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0); }; } @@ -157,14 +184,17 @@ void QuaternionParameterizationTestHelper(const double* x, const double* delta, const double* q_delta) { + const int kGlobalSize = 4; + const int kLocalSize = 3; + const double kTolerance = 1e-14; - double x_plus_delta_ref[4] = {0.0, 0.0, 0.0, 0.0}; + double x_plus_delta_ref[kGlobalSize] = {0.0, 0.0, 0.0, 0.0}; QuaternionProduct(q_delta, x, x_plus_delta_ref); - double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; - QuaternionParameterization param; - param.Plus(x, delta, x_plus_delta); - for (int i = 0; i < 4; ++i) { + double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0, 0.0}; + QuaternionParameterization parameterization; + parameterization.Plus(x, delta, x_plus_delta); + for (int i = 0; i < kGlobalSize; ++i) { EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); } @@ -177,23 +207,41 @@ EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance); double jacobian_ref[12]; - double zero_delta[3] = {0.0, 0.0, 0.0}; + double zero_delta[kLocalSize] = {0.0, 0.0, 0.0}; const double* parameters[2] = {x, zero_delta}; double* jacobian_array[2] = { NULL, jacobian_ref }; // Autodiff jacobian at delta_x = 0. - internal::AutoDiff<QuaternionPlus, double, 4, 3>::Differentiate( - QuaternionPlus(), parameters, 4, x_plus_delta, jacobian_array); + internal::AutoDiff<QuaternionPlus, + double, + kGlobalSize, + kLocalSize>::Differentiate(QuaternionPlus(), + parameters, + kGlobalSize, + x_plus_delta, + jacobian_array); double jacobian[12]; - param.ComputeJacobian(x, jacobian); + parameterization.ComputeJacobian(x, jacobian); for (int i = 0; i < 12; ++i) { EXPECT_TRUE(IsFinite(jacobian[i])); EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) << "Jacobian mismatch: i = " << i - << "\n Expected \n" << ConstMatrixRef(jacobian_ref, 4, 3) - << "\n Actual \n" << ConstMatrixRef(jacobian, 4, 3); + << "\n Expected \n" + << ConstMatrixRef(jacobian_ref, kGlobalSize, kLocalSize) + << "\n Actual \n" + << ConstMatrixRef(jacobian, kGlobalSize, kLocalSize); } + + Matrix global_matrix = Matrix::Random(10, kGlobalSize); + Matrix local_matrix = Matrix::Zero(10, kLocalSize); + parameterization.MultiplyByJacobian(x, + 10, + global_matrix.data(), + local_matrix.data()); + Matrix expected_local_matrix = + global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize); + EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0); } TEST(QuaternionParameterization, ZeroTest) {