Add IsParameterBlockConstant to the ceres::Problem class.

Change-Id: I7d0e828e81324443209c17fa54dd1d37605e5bfe

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

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) {

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);

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;

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];