Let Problem::SetParameterization be called more than once.

https://github.com/ceres-solver/ceres-solver/issues/501

Change-Id: Ia94e6e62553e97fa2052db2cbebc5c472e26a406
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index c19a4ef..2604027 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -1762,8 +1762,8 @@
    The local_parameterization is owned by the Problem by default. It
    is acceptable to set the same parameterization for multiple
    parameters; the destructor is careful to delete local
-   parameterizations only once. The local parameterization can only be
-   set once per parameter, and cannot be changed once set.
+   parameterizations only once. Calling `SetParameterization` with
+   `nullptr` will clear any previously set parameterization.
 
 .. function:: LocalParameterization* Problem::GetParameterization(const double* values) const
 
diff --git a/include/ceres/problem.h b/include/ceres/problem.h
index a4a6c8f..ba9b903 100644
--- a/include/ceres/problem.h
+++ b/include/ceres/problem.h
@@ -315,8 +315,8 @@
   // The local_parameterization is owned by the Problem by default. It
   // is acceptable to set the same parameterization for multiple
   // parameters; the destructor is careful to delete local
-  // parameterizations only once. The local parameterization can only
-  // be set once per parameter, and cannot be changed once set.
+  // parameterizations only once. Calling SetParameterization with
+  // nullptr will clear any previously set parameterization.
   void SetParameterization(double* values,
                            LocalParameterization* local_parameterization);
 
diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h
index 620c4ef..3b7ae49 100644
--- a/internal/ceres/parameter_block.h
+++ b/internal/ceres/parameter_block.h
@@ -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
@@ -38,6 +38,7 @@
 #include <memory>
 #include <string>
 #include <unordered_set>
+
 #include "ceres/array_utils.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/port.h"
@@ -166,23 +167,19 @@
                : local_parameterization_->LocalSize();
   }
 
-  // Set the parameterization. The parameterization can be set exactly once;
-  // multiple calls to set the parameterization to different values will crash.
-  // It is an error to pass nullptr for the parameterization. The parameter
-  // block does not take ownership of the parameterization.
+  // Set the parameterization. The parameter block does not take
+  // ownership of the parameterization.
   void SetParameterization(LocalParameterization* new_parameterization) {
-    CHECK(new_parameterization != nullptr)
-        << "nullptr 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_ == nullptr)
-        << "Can't re-set the local parameterization; it leads to "
-        << "ambiguous ownership. Current local parameterization is: "
-        << local_parameterization_;
+    if (new_parameterization == nullptr) {
+      local_parameterization_ = nullptr;
+      return;
+    }
 
     CHECK(new_parameterization->GlobalSize() == size_)
         << "Invalid parameterization for parameter block. The parameter block "
diff --git a/internal/ceres/parameter_block_test.cc b/internal/ceres/parameter_block_test.cc
index babd354..4b65c40 100644
--- a/internal/ceres/parameter_block_test.cc
+++ b/internal/ceres/parameter_block_test.cc
@@ -30,8 +30,8 @@
 
 #include "ceres/parameter_block.h"
 
-#include "gtest/gtest.h"
 #include "ceres/internal/eigen.h"
+#include "gtest/gtest.h"
 
 namespace ceres {
 namespace internal {
@@ -56,37 +56,6 @@
   parameter_block.SetParameterization(&subset);
 }
 
-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(nullptr), "nullptr");
-}
-
-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");
-}
-
-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(nullptr), "nullptr");
-}
-
 TEST(ParameterBlock, SetParameterizationDiesOnZeroLocalSize) {
   double x[3] = {1.0, 2.0, 3.0};
   ParameterBlock parameter_block(x, 3, -1);
@@ -100,7 +69,7 @@
 }
 
 TEST(ParameterBlock, SetLocalParameterizationAndNormalOperation) {
-  double x[3] = { 1.0, 2.0, 3.0 };
+  double x[3] = {1.0, 2.0, 3.0};
   ParameterBlock parameter_block(x, 3, -1);
   std::vector<int> indices;
   indices.push_back(1);
@@ -109,9 +78,7 @@
 
   // Ensure the local parameterization jacobian result is correctly computed.
   ConstMatrixRef local_parameterization_jacobian(
-      parameter_block.LocalParameterizationJacobian(),
-      3,
-      2);
+      parameter_block.LocalParameterizationJacobian(), 3, 2);
   ASSERT_EQ(1.0, local_parameterization_jacobian(0, 0));
   ASSERT_EQ(0.0, local_parameterization_jacobian(0, 1));
   ASSERT_EQ(0.0, local_parameterization_jacobian(1, 0));
@@ -121,7 +88,7 @@
 
   // Check that updating works as expected.
   double x_plus_delta[3];
-  double delta[2] = { 0.5, 0.3 };
+  double delta[2] = {0.5, 0.3};
   parameter_block.Plus(x, delta, x_plus_delta);
   ASSERT_EQ(1.5, x_plus_delta[0]);
   ASSERT_EQ(2.0, x_plus_delta[1]);
@@ -137,8 +104,7 @@
     LOG(FATAL) << "Shouldn't get called.";
     return true;
   }
-  bool ComputeJacobian(const double* x,
-                       double* jacobian) const final {
+  bool ComputeJacobian(const double* x, double* jacobian) const final {
     jacobian[0] = *x * 2;
     return true;
   }
@@ -149,7 +115,7 @@
 
 TEST(ParameterBlock, SetStateUpdatesLocalParameterizationJacobian) {
   TestParameterization test_parameterization;
-  double x[1] = { 1.0 };
+  double x[1] = {1.0};
   ParameterBlock parameter_block(x, 1, -1, &test_parameterization);
 
   EXPECT_EQ(2.0, *parameter_block.LocalParameterizationJacobian());
@@ -160,10 +126,10 @@
 }
 
 TEST(ParameterBlock, PlusWithNoLocalParameterization) {
-  double x[2] = { 1.0, 2.0 };
+  double x[2] = {1.0, 2.0};
   ParameterBlock parameter_block(x, 2, -1);
 
-  double delta[2] = { 0.2, 0.3 };
+  double delta[2] = {0.2, 0.3};
   double x_plus_delta[2];
   parameter_block.Plus(x, delta, x_plus_delta);
   EXPECT_EQ(1.2, x_plus_delta[0]);
@@ -173,9 +139,7 @@
 // Stops computing the jacobian after the first time.
 class BadLocalParameterization : public LocalParameterization {
  public:
-  BadLocalParameterization()
-      : calls_(0) {
-  }
+  BadLocalParameterization() : calls_(0) {}
 
   virtual ~BadLocalParameterization() {}
   bool Plus(const double* x,
@@ -193,8 +157,8 @@
     return true;
   }
 
-  int GlobalSize() const final { return 1;}
-  int LocalSize()  const final { return 1;}
+  int GlobalSize() const final { return 1; }
+  int LocalSize() const final { return 1; }
 
  private:
   mutable int calls_;
@@ -248,5 +212,40 @@
   EXPECT_EQ(x_plus_delta[1], -1.0);
 }
 
+TEST(ParameterBlock, ResetLocalParameterizationToNull) {
+  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);
+  parameter_block.SetParameterization(nullptr);
+  EXPECT_EQ(parameter_block.local_parameterization(), nullptr);
+}
+
+TEST(ParameterBlock, ResetLocalParameterizationToNotNull) {
+  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, SetNullLocalParameterization) {
+  double x[3] = {1.0, 2.0, 3.0};
+  ParameterBlock parameter_block(x, 3, -1);
+  EXPECT_EQ(parameter_block.local_parameterization(), nullptr);
+
+  parameter_block.SetParameterization(nullptr);
+  EXPECT_EQ(parameter_block.local_parameterization(), nullptr);
+}
+
 }  // namespace internal
 }  // namespace ceres
diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index 63dad12..7a36c2b 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.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
@@ -113,7 +113,7 @@
 void InitializeContext(Context* context,
                        ContextImpl** context_impl,
                        bool* context_impl_owned) {
-  if (context == NULL) {
+  if (context == nullptr) {
     *context_impl_owned = true;
     *context_impl = new ContextImpl;
   } else {
@@ -126,8 +126,8 @@
 
 ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values,
                                                        int size) {
-  CHECK(values != NULL) << "Null pointer passed to AddParameterBlock "
-                        << "for a parameter with size " << size;
+  CHECK(values != nullptr) << "Null pointer passed to AddParameterBlock "
+                           << "for a parameter with size " << size;
 
   // Ignore the request if there is a block for the given pointer already.
   ParameterMap::iterator it = parameter_block_map_.find(values);
@@ -216,7 +216,7 @@
   LossFunction* loss_function =
       const_cast<LossFunction*>(residual_block->loss_function());
   if (options_.loss_function_ownership == TAKE_OWNERSHIP &&
-      loss_function != NULL) {
+      loss_function != nullptr) {
     DecrementValueOrDeleteKey(loss_function, &loss_function_ref_count_);
   }
 
@@ -230,7 +230,7 @@
 // without doing a full scan.
 void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) {
   if (options_.local_parameterization_ownership == TAKE_OWNERSHIP &&
-      parameter_block->local_parameterization() != NULL) {
+      parameter_block->local_parameterization() != nullptr) {
     local_parameterizations_to_delete_.push_back(
         parameter_block->mutable_local_parameterization());
   }
@@ -361,7 +361,7 @@
   }
 
   if (options_.loss_function_ownership == TAKE_OWNERSHIP &&
-      loss_function != NULL) {
+      loss_function != nullptr) {
     ++loss_function_ref_count_[loss_function];
   }
 
@@ -375,7 +375,7 @@
 void ProblemImpl::AddParameterBlock(
     double* values, int size, LocalParameterization* local_parameterization) {
   ParameterBlock* parameter_block = InternalAddParameterBlock(values, size);
-  if (local_parameterization != NULL) {
+  if (local_parameterization != nullptr) {
     parameter_block->SetParameterization(local_parameterization);
   }
 }
@@ -519,6 +519,15 @@
                << "you can set its local parameterization.";
   }
 
+  // If the parameter block already has a local parameterization and
+  // we are to take ownership of local parameterizations, then add it
+  // to local_parameterizations_to_delete_ for eventual deletion.
+  if (parameter_block->local_parameterization_ &&
+      options_.local_parameterization_ownership == TAKE_OWNERSHIP) {
+    local_parameterizations_to_delete_.push_back(
+        parameter_block->local_parameterization_);
+  }
+
   parameter_block->SetParameterization(local_parameterization);
 }
 
diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h
index f366b7d..8bbe723 100644
--- a/internal/ceres/problem_impl.h
+++ b/internal/ceres/problem_impl.h
@@ -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
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index b779524..12e8dc2 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -2096,5 +2096,32 @@
             std::numeric_limits<double>::max());
 }
 
+TEST(Problem, SetParameterizationTwice) {
+  Problem problem;
+  double x[] = {1.0, 2.0, 3.0};
+  problem.AddParameterBlock(x, 3);
+  problem.SetParameterization(x, new SubsetParameterization(3, {1}));
+  EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3);
+  EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2);
+
+  problem.SetParameterization(x, new SubsetParameterization(3, {0, 1}));
+  EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3);
+  EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 1);
+}
+
+TEST(Problem, SetParameterizationAndThenClearItWithNull) {
+  Problem problem;
+  double x[] = {1.0, 2.0, 3.0};
+  problem.AddParameterBlock(x, 3);
+  problem.SetParameterization(x, new SubsetParameterization(3, {1}));
+  EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3);
+  EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2);
+
+  problem.SetParameterization(x, nullptr);
+  EXPECT_EQ(problem.GetParameterization(x), nullptr);
+  EXPECT_EQ(problem.ParameterBlockLocalSize(x), 3);
+  EXPECT_EQ(problem.ParameterBlockSize(x), 3);
+}
+
 }  // namespace internal
 }  // namespace ceres