Allow SubsetParameterization to hold all parameters constant
1. SubsetParameterization can now be constructed such that all
parameters are constant. This is required for it be used as part
of a ProductParameterization to hold a part of parameter block
constant. For example, a parameter block consisting of a rotation
as a quaternion and a translation vector can now have a local
parameterization where the translation part is constant and the
quaternion part has a QuaternionParameterization associated with it.
2. The check for the tangent space of a parameterization being
positive dimensional. We were not doing this check up till now
and the user could accidentally create parameterizations like this
and create a problem for themselves. This will ensure that even
though one can construct a SubsetParameterization where all
parameters are constant, you cannot actually use it as a local
parameterization for an entire parameter block. Which is how
it was before, but the check was inside the SubsetParameterization
constructor.
3. Added more tests and refactored existing tests to be more
granular.
Change-Id: Ic0184a1f30e3bd8a416b02341781a9d98e855ff7
diff --git a/internal/ceres/local_parameterization.cc b/internal/ceres/local_parameterization.cc
index c1e0cda..a6bf1f6 100644
--- a/internal/ceres/local_parameterization.cc
+++ b/internal/ceres/local_parameterization.cc
@@ -89,28 +89,17 @@
}
SubsetParameterization::SubsetParameterization(
- int size,
- const vector<int>& constant_parameters)
- : local_size_(size - constant_parameters.size()),
- constancy_mask_(size, 0) {
- CHECK_GT(constant_parameters.size(), 0)
- << "The set of constant parameters should contain at least "
- << "one element. If you do not wish to hold any parameters "
- << "constant, then do not use a SubsetParameterization";
-
+ int size, const vector<int>& constant_parameters)
+ : local_size_(size - constant_parameters.size()), constancy_mask_(size, 0) {
vector<int> constant = constant_parameters;
- sort(constant.begin(), constant.end());
+ std::sort(constant.begin(), constant.end());
+ CHECK_GE(constant.front(), 0)
+ << "Indices indicating constant parameter must be greater than zero.";
+ CHECK_LT(constant.back(), size)
+ << "Indices indicating constant parameter must be less than the size "
+ << "of the parameter block.";
CHECK(std::adjacent_find(constant.begin(), constant.end()) == constant.end())
<< "The set of constant parameters cannot contain duplicates";
- CHECK_LT(constant_parameters.size(), size)
- << "Number of parameters held constant should be less "
- << "than the size of the parameter block. If you wish "
- << "to hold the entire parameter block constant, then a "
- << "efficient way is to directly mark it as constant "
- << "instead of using a LocalParameterization to do so.";
- CHECK_GE(*min_element(constant.begin(), constant.end()), 0);
- CHECK_LT(*max_element(constant.begin(), constant.end()), size);
-
for (int i = 0; i < constant_parameters.size(); ++i) {
constancy_mask_[constant_parameters[i]] = 1;
}
@@ -131,6 +120,10 @@
bool SubsetParameterization::ComputeJacobian(const double* x,
double* jacobian) const {
+ if (local_size_ == 0) {
+ return true;
+ }
+
MatrixRef m(jacobian, constancy_mask_.size(), local_size_);
m.setZero();
for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) {
@@ -145,6 +138,10 @@
const int num_rows,
const double* global_matrix,
double* local_matrix) const {
+ if (local_size_ == 0) {
+ return true;
+ }
+
for (int row = 0; row < num_rows; ++row) {
for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) {
if (!constancy_mask_[col]) {
@@ -367,9 +364,9 @@
if (!param->ComputeJacobian(x + x_cursor, buffer.get())) {
return false;
}
-
jacobian.block(x_cursor, delta_cursor, global_size, local_size)
= MatrixRef(buffer.get(), global_size, local_size);
+
delta_cursor += local_size;
x_cursor += global_size;
}
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc
index d861a95..d3c11c5 100644
--- a/internal/ceres/local_parameterization_test.cc
+++ b/internal/ceres/local_parameterization_test.cc
@@ -75,28 +75,76 @@
EXPECT_EQ((local_matrix - global_matrix).norm(), 0.0);
}
-TEST(SubsetParameterization, DeathTests) {
+
+TEST(SubsetParameterization, NegativeParameterIndexDeathTest) {
std::vector<int> constant_parameters;
- EXPECT_DEATH_IF_SUPPORTED(
- SubsetParameterization parameterization(1, constant_parameters),
- "at least");
-
- constant_parameters.push_back(0);
- EXPECT_DEATH_IF_SUPPORTED(
- SubsetParameterization parameterization(1, constant_parameters),
- "Number of parameters");
-
- constant_parameters.push_back(1);
+ constant_parameters.push_back(-1);
EXPECT_DEATH_IF_SUPPORTED(
SubsetParameterization parameterization(2, constant_parameters),
- "Number of parameters");
+ "greater than zero");
+}
+TEST(SubsetParameterization, GreaterThanSizeParameterIndexDeathTest) {
+ std::vector<int> constant_parameters;
+ constant_parameters.push_back(2);
+ EXPECT_DEATH_IF_SUPPORTED(
+ SubsetParameterization parameterization(2, constant_parameters),
+ "less than the size");
+}
+
+TEST(SubsetParameterization, DuplicateParametersDeathTest) {
+ std::vector<int> constant_parameters;
+ constant_parameters.push_back(1);
constant_parameters.push_back(1);
EXPECT_DEATH_IF_SUPPORTED(
SubsetParameterization parameterization(2, constant_parameters),
"duplicates");
}
+TEST(SubsetParameterization,
+ ProductParameterizationWithZeroLocalSizeSubsetParameterization1) {
+ std::vector<int> constant_parameters;
+ constant_parameters.push_back(0);
+ LocalParameterization* subset_param =
+ new SubsetParameterization(1, constant_parameters);
+ LocalParameterization* identity_param = new IdentityParameterization(2);
+ ProductParameterization product_param(subset_param, identity_param);
+ EXPECT_EQ(product_param.GlobalSize(), 3);
+ EXPECT_EQ(product_param.LocalSize(), 2);
+ double x[] = {1.0, 1.0, 1.0};
+ double delta[] = {2.0, 3.0};
+ double x_plus_delta[] = {0.0, 0.0, 0.0};
+ EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));
+ EXPECT_EQ(x_plus_delta[0], x[0]);
+ EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]);
+ EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]);
+
+ Matrix actual_jacobian(3, 2);
+ EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));
+}
+
+TEST(SubsetParameterization,
+ ProductParameterizationWithZeroLocalSizeSubsetParameterization2) {
+ std::vector<int> constant_parameters;
+ constant_parameters.push_back(0);
+ LocalParameterization* subset_param =
+ new SubsetParameterization(1, constant_parameters);
+ LocalParameterization* identity_param = new IdentityParameterization(2);
+ ProductParameterization product_param(identity_param, subset_param);
+ EXPECT_EQ(product_param.GlobalSize(), 3);
+ EXPECT_EQ(product_param.LocalSize(), 2);
+ double x[] = {1.0, 1.0, 1.0};
+ double delta[] = {2.0, 3.0};
+ double x_plus_delta[] = {0.0, 0.0, 0.0};
+ EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));
+ EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]);
+ EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]);
+ EXPECT_EQ(x_plus_delta[2], x[2]);
+
+ Matrix actual_jacobian(3, 2);
+ EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));
+}
+
TEST(SubsetParameterization, NormalFunctionTest) {
const int kGlobalSize = 4;
const int kLocalSize = 3;
diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h
index cb7140d..8e21553 100644
--- a/internal/ceres/parameter_block.h
+++ b/internal/ceres/parameter_block.h
@@ -161,25 +161,34 @@
// does not take ownership of the parameterization.
void SetParameterization(LocalParameterization* new_parameterization) {
CHECK(new_parameterization != NULL) << "NULL parameterization invalid.";
+ // Nothing to do if the new parameterization is the same as the
+ // old parameterization.
+ if (new_parameterization == local_parameterization_) {
+ return;
+ }
+
+ CHECK(local_parameterization_ == NULL)
+ << "Can't re-set the local parameterization; it leads to "
+ << "ambiguous ownership. Current local parameterization is: "
+ << local_parameterization_;
+
CHECK(new_parameterization->GlobalSize() == size_)
<< "Invalid parameterization for parameter block. The parameter block "
<< "has size " << size_ << " while the parameterization has a global "
<< "size of " << new_parameterization->GlobalSize() << ". Did you "
<< "accidentally use the wrong parameter block or parameterization?";
- if (new_parameterization != local_parameterization_) {
- CHECK(local_parameterization_ == NULL)
- << "Can't re-set the local parameterization; it leads to "
- << "ambiguous ownership.";
- local_parameterization_ = new_parameterization;
- local_parameterization_jacobian_.reset(
- new double[local_parameterization_->GlobalSize() *
- local_parameterization_->LocalSize()]);
- CHECK(UpdateLocalParameterizationJacobian())
- << "Local parameterization Jacobian computation failed for x: "
- << ConstVectorRef(state_, Size()).transpose();
- } else {
- // Ignore the case that the parameterizations match.
- }
+
+ CHECK_GT(new_parameterization->LocalSize(), 0)
+ << "Invalid parameterization. Parameterizations must have a positive "
+ << "dimensional tangent space.";
+
+ local_parameterization_ = new_parameterization;
+ local_parameterization_jacobian_.reset(
+ new double[local_parameterization_->GlobalSize() *
+ local_parameterization_->LocalSize()]);
+ CHECK(UpdateLocalParameterizationJacobian())
+ << "Local parameterization Jacobian computation failed for x: "
+ << ConstVectorRef(state_, Size()).transpose();
}
void SetUpperBound(int index, double upper_bound) {
diff --git a/internal/ceres/parameter_block_test.cc b/internal/ceres/parameter_block_test.cc
index e1d3bb3..164bb4c 100644
--- a/internal/ceres/parameter_block_test.cc
+++ b/internal/ceres/parameter_block_test.cc
@@ -36,37 +36,76 @@
namespace ceres {
namespace internal {
-TEST(ParameterBlock, SetLocalParameterization) {
- double x[3] = { 1.0, 2.0, 3.0 };
+TEST(ParameterBlock, SetLocalParameterizationDiesOnSizeMismatch) {
+ double x[3] = {1.0, 2.0, 3.0};
ParameterBlock parameter_block(x, 3, -1);
-
- // The indices to set constant within the parameter block (used later).
std::vector<int> indices;
indices.push_back(1);
-
- // Can't set the parameterization if the sizes don't match.
SubsetParameterization subset_wrong_size(4, indices);
EXPECT_DEATH_IF_SUPPORTED(
parameter_block.SetParameterization(&subset_wrong_size), "global");
+}
- // Can't set parameterization to NULL from NULL.
- EXPECT_DEATH_IF_SUPPORTED
- (parameter_block.SetParameterization(NULL), "NULL");
-
- // Now set the parameterization.
+TEST(ParameterBlock, SetLocalParameterizationWithSameExistingParameterization) {
+ double x[3] = {1.0, 2.0, 3.0};
+ ParameterBlock parameter_block(x, 3, -1);
+ std::vector<int> indices;
+ indices.push_back(1);
SubsetParameterization subset(3, indices);
parameter_block.SetParameterization(&subset);
-
- // Re-setting the parameterization to the same value is supported.
parameter_block.SetParameterization(&subset);
+}
- // Can't set parameterization to NULL from another parameterization.
+TEST(ParameterBlock, SetLocalParameterizationDiesWhenResettingToNull) {
+ double x[3] = {1.0, 2.0, 3.0};
+ ParameterBlock parameter_block(x, 3, -1);
+ std::vector<int> indices;
+ indices.push_back(1);
+ SubsetParameterization subset(3, indices);
+ parameter_block.SetParameterization(&subset);
EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL");
+}
- // Can't set the parameterization more than once.
+TEST(ParameterBlock,
+ SetLocalParameterizationDiesWhenResettingToDifferentParameterization) {
+ double x[3] = {1.0, 2.0, 3.0};
+ ParameterBlock parameter_block(x, 3, -1);
+ std::vector<int> indices;
+ indices.push_back(1);
+ SubsetParameterization subset(3, indices);
+ parameter_block.SetParameterization(&subset);
SubsetParameterization subset_different(3, indices);
- EXPECT_DEATH_IF_SUPPORTED
- (parameter_block.SetParameterization(&subset_different), "re-set");
+ EXPECT_DEATH_IF_SUPPORTED(
+ parameter_block.SetParameterization(&subset_different), "re-set");
+}
+
+TEST(ParameterBlock, SetLocalParameterizationDiesOnNullParameterization) {
+ double x[3] = {1.0, 2.0, 3.0};
+ ParameterBlock parameter_block(x, 3, -1);
+ std::vector<int> indices;
+ indices.push_back(1);
+ EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL");
+}
+
+TEST(ParameterBlock, SetParameterizationDiesOnZeroLocalSize) {
+ double x[3] = {1.0, 2.0, 3.0};
+ ParameterBlock parameter_block(x, 3, -1);
+ std::vector<int> indices;
+ indices.push_back(0);
+ indices.push_back(1);
+ indices.push_back(2);
+ SubsetParameterization subset(3, indices);
+ EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(&subset),
+ "positive dimensional tangent");
+}
+
+TEST(ParameterBlock, SetLocalParameterizationAndNormalOperation) {
+ double x[3] = { 1.0, 2.0, 3.0 };
+ ParameterBlock parameter_block(x, 3, -1);
+ std::vector<int> indices;
+ indices.push_back(1);
+ SubsetParameterization subset(3, indices);
+ parameter_block.SetParameterization(&subset);
// Ensure the local parameterization jacobian result is correctly computed.
ConstMatrixRef local_parameterization_jacobian(
