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) {
