Allow LocalParameterizations to have zero local size.
Local parameterizations with zero tangent/local size will cause the
corresponding parameter block to be treated as constant.
https://github.com/ceres-solver/ceres-solver/issues/347
Change-Id: I554a2acc420f5dd9d0cc7f97b691877eb057b2c0
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index 0883c6d..b95796b 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -1776,7 +1776,11 @@
.. function:: bool Problem::IsParameterBlockConstant(const double* values) const
- Returns true if a parameter block is set constant, and false otherwise.
+ Returns ``true`` if a parameter block is set constant, and false
+ otherwise. A parameter block may be set constant in two
+ ways. Either by calling ``SetParameterBlockConstant`` or by
+ associating a ``LocalParameterization`` with a zero dimensional
+ tangent space with it.
.. function:: void Problem::SetParameterization(double* values, LocalParameterization* local_parameterization)
diff --git a/include/ceres/problem.h b/include/ceres/problem.h
index e43616e..977a708 100644
--- a/include/ceres/problem.h
+++ b/include/ceres/problem.h
@@ -308,7 +308,11 @@
// Allow the indicated parameter block to vary during optimization.
void SetParameterBlockVariable(double* values);
- // Returns true if a parameter block is set constant, and false otherwise.
+ // Returns true if a parameter block is set constant, and false
+ // otherwise. A parameter block may be set constant in two
+ // ways. Either by calling SetParameterBlockConstant or by
+ // associating a LocalParameterization with a zero dimensional
+ // tangent space with it.
bool IsParameterBlockConstant(const double* values) const;
// Set the local parameterization for one of the parameter blocks.
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc
index ad5ffe6..34a36c6 100644
--- a/internal/ceres/covariance_test.cc
+++ b/internal/ceres/covariance_test.cc
@@ -37,6 +37,7 @@
#include <memory>
#include <utility>
+#include "ceres/autodiff_cost_function.h"
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/cost_function.h"
#include "ceres/covariance_impl.h"
@@ -1194,6 +1195,87 @@
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
+struct LinearCostFunction {
+ template <typename T>
+ bool operator()(const T* x, const T* y, T* residual) const {
+ residual[0] = T(10.0) - *x;
+ residual[1] = T(5.0) - *y;
+ return true;
+ }
+ static CostFunction* Create() {
+ return new AutoDiffCostFunction<LinearCostFunction, 2, 1, 1>(
+ new LinearCostFunction);
+ }
+};
+
+TEST(Covariance, ZeroSizedLocalParameterizationGetCovariance) {
+ double x = 0.0;
+ double y = 1.0;
+ Problem problem;
+ problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y);
+ problem.SetParameterization(&y, new SubsetParameterization(1, {0}));
+ // J = [-1 0]
+ // [ 0 0]
+ Covariance::Options options;
+ options.algorithm_type = DENSE_SVD;
+ Covariance covariance(options);
+ vector<pair<const double*, const double*>> covariance_blocks;
+ covariance_blocks.push_back(std::make_pair(&x, &x));
+ covariance_blocks.push_back(std::make_pair(&x, &y));
+ covariance_blocks.push_back(std::make_pair(&y, &x));
+ covariance_blocks.push_back(std::make_pair(&y, &y));
+ EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem));
+
+ double value = -1;
+ covariance.GetCovarianceBlock(&x, &x, &value);
+ EXPECT_NEAR(value, 1.0, std::numeric_limits<double>::epsilon());
+
+ value = -1;
+ covariance.GetCovarianceBlock(&x, &y, &value);
+ EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon());
+
+ value = -1;
+ covariance.GetCovarianceBlock(&y, &x, &value);
+ EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon());
+
+ value = -1;
+ covariance.GetCovarianceBlock(&y, &y, &value);
+ EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon());
+}
+
+TEST(Covariance, ZeroSizedLocalParameterizationGetCovarianceInTangentSpace) {
+ double x = 0.0;
+ double y = 1.0;
+ Problem problem;
+ problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y);
+ problem.SetParameterization(&y, new SubsetParameterization(1, {0}));
+ // J = [-1 0]
+ // [ 0 0]
+ Covariance::Options options;
+ options.algorithm_type = DENSE_SVD;
+ Covariance covariance(options);
+ vector<pair<const double*, const double*>> covariance_blocks;
+ covariance_blocks.push_back(std::make_pair(&x, &x));
+ covariance_blocks.push_back(std::make_pair(&x, &y));
+ covariance_blocks.push_back(std::make_pair(&y, &x));
+ covariance_blocks.push_back(std::make_pair(&y, &y));
+ EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem));
+
+ double value = -1;
+ covariance.GetCovarianceBlockInTangentSpace(&x, &x, &value);
+ EXPECT_NEAR(value, 1.0, std::numeric_limits<double>::epsilon());
+
+ value = -1;
+ // The following three calls, should not touch this value, since the
+ // tangent space is of size zero
+ covariance.GetCovarianceBlockInTangentSpace(&x, &y, &value);
+ EXPECT_EQ(value, -1);
+ covariance.GetCovarianceBlockInTangentSpace(&y, &x, &value);
+ EXPECT_EQ(value, -1);
+ covariance.GetCovarianceBlockInTangentSpace(&y, &y, &value);
+ EXPECT_EQ(value, -1);
+}
+
class LargeScaleCovarianceTest : public ::testing::Test {
protected:
void SetUp() final {
diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h
index 3b7ae49..d8432c7 100644
--- a/internal/ceres/parameter_block.h
+++ b/internal/ceres/parameter_block.h
@@ -124,7 +124,6 @@
// Set this parameter block to vary or not.
void SetConstant() { is_set_constant_ = true; }
void SetVarying() { is_set_constant_ = false; }
- bool IsSetConstantByUser() const { return is_set_constant_; }
bool IsConstant() const { return (is_set_constant_ || LocalSize() == 0); }
double UpperBound(int index) const {
@@ -187,9 +186,9 @@
<< "size of " << new_parameterization->GlobalSize() << ". Did you "
<< "accidentally use the wrong parameter block or parameterization?";
- CHECK_GT(new_parameterization->LocalSize(), 0)
+ CHECK_GE(new_parameterization->LocalSize(), 0)
<< "Invalid parameterization. Parameterizations must have a "
- << "positive dimensional tangent space.";
+ << "non-negative dimensional tangent space.";
local_parameterization_ = new_parameterization;
local_parameterization_jacobian_.reset(
diff --git a/internal/ceres/parameter_block_test.cc b/internal/ceres/parameter_block_test.cc
index 4b65c40..a5a4230 100644
--- a/internal/ceres/parameter_block_test.cc
+++ b/internal/ceres/parameter_block_test.cc
@@ -36,7 +36,7 @@
namespace ceres {
namespace internal {
-TEST(ParameterBlock, SetLocalParameterizationDiesOnSizeMismatch) {
+TEST(ParameterBlock, SetParameterizationDiesOnSizeMismatch) {
double x[3] = {1.0, 2.0, 3.0};
ParameterBlock parameter_block(x, 3, -1);
std::vector<int> indices;
@@ -46,7 +46,7 @@
parameter_block.SetParameterization(&subset_wrong_size), "global");
}
-TEST(ParameterBlock, SetLocalParameterizationWithSameExistingParameterization) {
+TEST(ParameterBlock, SetParameterizationWithSameExistingParameterization) {
double x[3] = {1.0, 2.0, 3.0};
ParameterBlock parameter_block(x, 3, -1);
std::vector<int> indices;
@@ -56,19 +56,34 @@
parameter_block.SetParameterization(&subset);
}
-TEST(ParameterBlock, SetParameterizationDiesOnZeroLocalSize) {
+TEST(ParameterBlock, SetParameterizationAllowsResettingToNull) {
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");
+ parameter_block.SetParameterization(&subset);
+ EXPECT_EQ(parameter_block.local_parameterization(), &subset);
+ parameter_block.SetParameterization(nullptr);
+ EXPECT_EQ(parameter_block.local_parameterization(), nullptr);
}
-TEST(ParameterBlock, SetLocalParameterizationAndNormalOperation) {
+TEST(ParameterBlock,
+ SetParameterizationAllowsResettingToDifferentParameterization) {
+ 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_EQ(parameter_block.local_parameterization(), &subset);
+
+ SubsetParameterization subset_different(3, indices);
+ parameter_block.SetParameterization(&subset_different);
+ EXPECT_EQ(parameter_block.local_parameterization(), &subset_different);
+}
+
+TEST(ParameterBlock, SetParameterizationAndNormalOperation) {
double x[3] = {1.0, 2.0, 3.0};
ParameterBlock parameter_block(x, 3, -1);
std::vector<int> indices;
diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index 7a36c2b..6cc4d33 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.cc
@@ -493,8 +493,7 @@
CHECK(parameter_block != nullptr)
<< "Parameter block not found: " << values << ". You must add the "
<< "parameter block to the problem before it can be queried.";
-
- return parameter_block->IsSetConstantByUser();
+ return parameter_block->IsConstant();
}
void ProblemImpl::SetParameterBlockVariable(double* values) {
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index 12e8dc2..f2b5da5 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -2123,5 +2123,18 @@
EXPECT_EQ(problem.ParameterBlockSize(x), 3);
}
+TEST(Solver, ZeroSizedLocalParameterizationHoldsParameterBlockConstant) {
+ double x = 0.0;
+ double y = 1.0;
+ Problem problem;
+ problem.AddResidualBlock(new BinaryCostFunction(1, 1, 1), nullptr, &x, &y);
+ problem.SetParameterization(&y, new SubsetParameterization(1, {0}));
+ // Zero dimensional tangent space means that the block is
+ // effectively constant, but because the user did not mark it
+ // constant explicitly, the user will not see it as constant when
+ // querying IsParameterBlockConstant.
+ EXPECT_TRUE(problem.IsParameterBlockConstant(&y));
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/solver_test.cc b/internal/ceres/solver_test.cc
index 601c1e4..58148b2 100644
--- a/internal/ceres/solver_test.cc
+++ b/internal/ceres/solver_test.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2015 Google Inc. All rights reserved.
+// Copyright 2019 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -30,16 +30,18 @@
#include "ceres/solver.h"
+#include <cmath>
#include <limits>
#include <memory>
-#include <cmath>
#include <vector>
-#include "gtest/gtest.h"
-#include "ceres/evaluation_callback.h"
+
#include "ceres/autodiff_cost_function.h"
-#include "ceres/sized_cost_function.h"
+#include "ceres/evaluation_callback.h"
+#include "ceres/local_parameterization.h"
#include "ceres/problem.h"
#include "ceres/problem_impl.h"
+#include "ceres/sized_cost_function.h"
+#include "gtest/gtest.h"
namespace ceres {
namespace internal {
@@ -61,8 +63,8 @@
}
struct QuadraticCostFunctor {
- template <typename T> bool operator()(const T* const x,
- T* residual) const {
+ template <typename T>
+ bool operator()(const T* const x, T* residual) const {
residual[0] = T(5.0) - *x;
return true;
}
@@ -74,14 +76,14 @@
};
struct RememberingCallback : public IterationCallback {
- explicit RememberingCallback(double *x) : calls(0), x(x) {}
+ explicit RememberingCallback(double* x) : calls(0), x(x) {}
virtual ~RememberingCallback() {}
CallbackReturnType operator()(const IterationSummary& summary) final {
x_values.push_back(*x);
return SOLVER_CONTINUE;
}
int calls;
- double *x;
+ double* x;
std::vector<double> x_values;
};
@@ -89,8 +91,8 @@
virtual ~NoOpEvaluationCallback() {}
void PrepareForEvaluation(bool evaluate_jacobians,
bool new_evaluation_point) final {
- (void) evaluate_jacobians;
- (void) new_evaluation_point;
+ (void)evaluate_jacobians;
+ (void)new_evaluation_point;
}
};
@@ -114,8 +116,8 @@
// First: update_state_every_iteration=false, evaluation_callback=nullptr.
Solve(options, &problem, &summary);
- num_iterations = summary.num_successful_steps +
- summary.num_unsuccessful_steps;
+ num_iterations =
+ summary.num_successful_steps + summary.num_unsuccessful_steps;
EXPECT_GT(num_iterations, 1);
for (int i = 0; i < callback.x_values.size(); ++i) {
EXPECT_EQ(50.0, callback.x_values[i]);
@@ -126,8 +128,8 @@
options.update_state_every_iteration = true;
callback.x_values.clear();
Solve(options, &problem, &summary);
- num_iterations = summary.num_successful_steps +
- summary.num_unsuccessful_steps;
+ num_iterations =
+ summary.num_successful_steps + summary.num_unsuccessful_steps;
EXPECT_GT(num_iterations, 1);
EXPECT_EQ(original_x, callback.x_values[0]);
EXPECT_NE(original_x, callback.x_values[1]);
@@ -158,8 +160,8 @@
options.update_state_every_iteration = true;
callback.x_values.clear();
Solve(options, &problem, &summary);
- num_iterations = summary.num_successful_steps +
- summary.num_unsuccessful_steps;
+ num_iterations =
+ summary.num_successful_steps + summary.num_unsuccessful_steps;
EXPECT_GT(num_iterations, 1);
EXPECT_EQ(original_x, callback.x_values[0]);
EXPECT_NE(original_x, callback.x_values[1]);
@@ -169,8 +171,8 @@
options.update_state_every_iteration = false;
callback.x_values.clear();
Solve(options, &problem, &summary);
- num_iterations = summary.num_successful_steps +
- summary.num_unsuccessful_steps;
+ num_iterations =
+ summary.num_successful_steps + summary.num_unsuccessful_steps;
EXPECT_GT(num_iterations, 1);
EXPECT_EQ(original_x, callback.x_values[0]);
EXPECT_NE(original_x, callback.x_values[1]);
@@ -199,20 +201,17 @@
EXPECT_EQ(summary.termination_type, CONVERGENCE);
}
-
// The parameters must be in separate blocks so that they can be individually
// set constant or not.
struct Quadratic4DCostFunction {
- template <typename T> bool operator()(const T* const x,
- const T* const y,
- const T* const z,
- const T* const w,
- T* residual) const {
+ template <typename T>
+ bool operator()(const T* const x,
+ const T* const y,
+ const T* const z,
+ const T* const w,
+ T* residual) const {
// A 4-dimension axis-aligned quadratic.
- residual[0] = T(10.0) - *x +
- T(20.0) - *y +
- T(30.0) - *z +
- T(40.0) - *w;
+ residual[0] = T(10.0) - *x + T(20.0) - *y + T(30.0) - *z + T(40.0) - *w;
return true;
}
@@ -423,7 +422,8 @@
EXPECT_FALSE(options.IsValid(&message));
}
-TEST(Solver, IterativeSchurWithClusterTridiagonalPerconditionerNoSparseLibrary) {
+TEST(Solver,
+ IterativeSchurWithClusterTridiagonalPerconditionerNoSparseLibrary) {
Solver::Options options;
options.sparse_linear_algebra_library_type = NO_SPARSE;
options.linear_solver_type = ITERATIVE_SCHUR;
@@ -458,9 +458,8 @@
EXPECT_TRUE(options.IsValid(&message));
options.linear_solver_type = SPARSE_SCHUR;
-#if defined(CERES_NO_SUITESPARSE) && \
- defined(CERES_NO_CXSPARSE) && \
- !defined(CERES_USE_EIGEN_SPARSE)
+#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) && \
+ !defined(CERES_USE_EIGEN_SPARSE)
EXPECT_FALSE(options.IsValid(&message));
#else
EXPECT_TRUE(options.IsValid(&message));
@@ -500,5 +499,40 @@
EXPECT_EQ(summary.iterations.size(), 0);
}
+struct LinearCostFunction {
+ template <typename T>
+ bool operator()(const T* x, const T* y, T* residual) const {
+ residual[0] = T(10.0) - *x;
+ residual[1] = T(5.0) - *y;
+ return true;
+ }
+ static CostFunction* Create() {
+ return new AutoDiffCostFunction<LinearCostFunction, 2, 1, 1>(
+ new LinearCostFunction);
+ }
+};
+
+TEST(Solver, ZeroSizedLocalParameterizationHoldsParameterBlockConstant) {
+ double x = 0.0;
+ double y = 1.0;
+ Problem problem;
+ problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y);
+ problem.SetParameterization(&y, new SubsetParameterization(1, {0}));
+ // Zero dimensional tangent space means that the block is
+ // effectively constant, but because the user did not mark it
+ // constant explicitly, the user will not see it as constant when
+ // querying IsParameterBlockConstant.
+ EXPECT_TRUE(problem.IsParameterBlockConstant(&y));
+ Solver::Options options;
+ options.function_tolerance = 0.0;
+ options.gradient_tolerance = 0.0;
+ options.parameter_tolerance = 0.0;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ EXPECT_EQ(summary.termination_type, CONVERGENCE);
+ EXPECT_NEAR(x, 10.0, 1e-7);
+ EXPECT_EQ(y, 1.0);
+}
+
} // namespace internal
} // namespace ceres
