commit d61e94da5225217cab7b4f93b72f97055094681f
author Thomas Schneider <schneith@ethz.ch> Wed Apr 06 10:40:29 2016 +0200
committer Thomas Schneider <schneith@ethz.ch> Wed Apr 06 23:52:18 2016 +0200
tree 6b6e286f7b7a5bd21bb9fd3fb205c5ff7f6b81fd
parent 77d94b34741574e958a417561702d6093fba87fb

Add IsParameterBlockConstant to the ceres::Problem class.

Change-Id: I7d0e828e81324443209c17fa54dd1d37605e5bfe

internal/ceres/problem.cc

diff --git a/internal/ceres/problem.cc b/internal/ceres/problem.cc
index 03b7d6a..730ce64 100644
--- a/internal/ceres/problem.cc
+++ b/internal/ceres/problem.cc
@@ -174,6 +174,10 @@
   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

diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index 7137616..4abea8b 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.cc
@@ -573,6 +573,16 @@
   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

diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h
index f42bde6..a4689c3 100644
--- a/internal/ceres/problem_impl.h
+++ b/internal/ceres/problem_impl.h
@@ -128,6 +128,8 @@
 
   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

diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index 31bf2d3..08d4b04 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -523,6 +523,41 @@
                             "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];