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