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

docs/source/nnls_modeling.rst

@@ -1776,7 +1776,11 @@ Instances
 
 .. 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)
 

include/ceres/problem.h

@@ -308,7 +308,11 @@ class CERES_EXPORT Problem {
   // 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.

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 @@ TEST_F(RankDeficientCovarianceTest, AutomaticTruncation) {
   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 {

internal/ceres/parameter_block.h

@@ -124,7 +124,6 @@ class ParameterBlock {
   // 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 @@ class ParameterBlock {
         << "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(

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 @@ TEST(ParameterBlock, SetLocalParameterizationDiesOnSizeMismatch) {
       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 @@ TEST(ParameterBlock, SetLocalParameterizationWithSameExistingParameterization) {
   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;

internal/ceres/problem_impl.cc

@@ -493,8 +493,7 @@ bool ProblemImpl::IsParameterBlockConstant(const double* values) const {
   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) {

internal/ceres/problem_test.cc

@@ -2123,5 +2123,18 @@ TEST(Problem, SetParameterizationAndThenClearItWithNull) {
   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

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 @@ TEST(SolverOptions, DefaultLineSearchOptionsAreValid) {
 }
 
 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 QuadraticCostFunctor {
 };
 
 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 @@ struct NoOpEvaluationCallback : EvaluationCallback {
   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 @@ TEST(Solver, UpdateStateEveryIterationOptionNoEvaluationCallback) {
 
   // 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 @@ TEST(Solver, UpdateStateEveryIterationOptionNoEvaluationCallback) {
   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 @@ TEST(Solver, UpdateStateEveryIterationOptionWithEvaluationCallback) {
   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 @@ TEST(Solver, UpdateStateEveryIterationOptionWithEvaluationCallback) {
   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 @@ TEST(Solver, CantMixEvaluationCallbackWithInnerIterations) {
   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 @@ TEST(Solver, IterativeSchurWithClusterJacobiPerconditionerNoSparseLibrary) {
   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 @@ TEST(Solver, LinearSolverTypeNormalOperation) {
   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 @@ TEST(Solver, FixedCostForConstantProblem) {
   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