From d61e94da5225217cab7b4f93b72f97055094681f Mon Sep 17 00:00:00 2001
From: Thomas Schneider <schneith@ethz.ch>
Date: Wed, 6 Apr 2016 10:40:29 +0200
Subject: [PATCH] Add IsParameterBlockConstant to the ceres::Problem class.

Change-Id: I7d0e828e81324443209c17fa54dd1d37605e5bfe
---
 include/ceres/problem.h        |  3 +++
 internal/ceres/problem.cc      |  4 ++++
 internal/ceres/problem_impl.cc | 10 ++++++++++
 internal/ceres/problem_impl.h  |  2 ++
 internal/ceres/problem_test.cc | 35 ++++++++++++++++++++++++++++++++++
 5 files changed, 54 insertions(+)

diff --git a/include/ceres/problem.h b/include/ceres/problem.h
index 409274c62..14bf6c2e5 100644
--- a/include/ceres/problem.h
+++ b/include/ceres/problem.h
@@ -309,6 +309,9 @@ 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.
+  bool IsParameterBlockConstant(double* values) const;
+
   // Set the local parameterization for one of the parameter blocks.
   // The local_parameterization is owned by the Problem by default. It
   // is acceptable to set the same parameterization for multiple
diff --git a/internal/ceres/problem.cc b/internal/ceres/problem.cc
index 03b7d6afa..730ce6420 100644
--- a/internal/ceres/problem.cc
+++ b/internal/ceres/problem.cc
@@ -174,6 +174,10 @@ void Problem::SetParameterBlockVariable(double* values) {
   problem_impl_->SetParameterBlockVariable(values);
 }
 
+bool Problem::IsParameterBlockConstant(double* values) const {
+  return problem_impl_->IsParameterBlockConstant(values);
+}
+
 void Problem::SetParameterization(
     double* values,
     LocalParameterization* local_parameterization) {
diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index 7137616e4..4abea8b33 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.cc
@@ -573,6 +573,16 @@ void ProblemImpl::SetParameterBlockConstant(double* values) {
   parameter_block->SetConstant();
 }
 
+bool ProblemImpl::IsParameterBlockConstant(double* values) const {
+  const ParameterBlock* parameter_block =
+      FindWithDefault(parameter_block_map_, values, NULL);
+  CHECK(parameter_block != NULL)
+    << "Parameter block not found: " << values << ". You must add the "
+    << "parameter block to the problem before it can be queried.";
+
+  return parameter_block->IsConstant();
+}
+
 void ProblemImpl::SetParameterBlockVariable(double* values) {
   ParameterBlock* parameter_block =
       FindWithDefault(parameter_block_map_, values, NULL);
diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h
index f42bde6c7..a4689c362 100644
--- a/internal/ceres/problem_impl.h
+++ b/internal/ceres/problem_impl.h
@@ -128,6 +128,8 @@ class ProblemImpl {
 
   void SetParameterBlockConstant(double* values);
   void SetParameterBlockVariable(double* values);
+  bool IsParameterBlockConstant(double* values) const;
+
   void SetParameterization(double* values,
                            LocalParameterization* local_parameterization);
   const LocalParameterization* GetParameterization(double* values) const;
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index 31bf2d38b..08d4b0484 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -523,6 +523,41 @@ TEST(Problem, SetParameterBlockVariableWithUnknownPtrDies) {
                             "Parameter block not found:");
 }
 
+TEST(Problem, IsParameterBlockConstant) {
+  double x1[3];
+  double x2[3];
+
+  Problem problem;
+  problem.AddParameterBlock(x1, 3);
+  problem.AddParameterBlock(x2, 3);
+
+  EXPECT_FALSE(problem.IsParameterBlockConstant(x1));
+  EXPECT_FALSE(problem.IsParameterBlockConstant(x2));
+
+  problem.SetParameterBlockConstant(x1);
+  EXPECT_TRUE(problem.IsParameterBlockConstant(x1));
+  EXPECT_FALSE(problem.IsParameterBlockConstant(x2));
+
+  problem.SetParameterBlockConstant(x2);
+  EXPECT_TRUE(problem.IsParameterBlockConstant(x1));
+  EXPECT_TRUE(problem.IsParameterBlockConstant(x2));
+
+  problem.SetParameterBlockVariable(x1);
+  EXPECT_FALSE(problem.IsParameterBlockConstant(x1));
+  EXPECT_TRUE(problem.IsParameterBlockConstant(x2));
+}
+
+TEST(Problem, IsParameterBlockConstantWithUnknownPtrDies) {
+  double x[3];
+  double y[2];
+
+  Problem problem;
+  problem.AddParameterBlock(x, 3);
+
+  EXPECT_DEATH_IF_SUPPORTED(problem.IsParameterBlockConstant(y),
+                            "Parameter block not found:");
+}
+
 TEST(Problem, SetLocalParameterizationWithUnknownPtrDies) {
   double x[3];
   double y[2];