Restore the state of the Problem after a call to Evaluate.

Calling Problem::Evaluate mutates the state of the parameter blocks.
In particular, depending on the set and order of parameter blocks
passed to the evaluate call, it will change the internal indexing
used by the Program object used by ProblemImpl. This needs to be
undone before Evaluate returns, otherwise the Problem object
is in an invalid state.

To help with testing and debugging in the future, a new method
Program::IsValid has been added which checks whether the problem
has its parameter and residual blocks in the right state.

Thanks to Stefan Leutenegger for reporting this.

Change-Id: I209b486a31433f0cbb58b570047649eca6d42b56

internal/ceres/problem_impl.cc

@@ -638,6 +638,9 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
     for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
       variable_parameter_blocks[i]->SetVarying();
     }
+
+    program_->SetParameterBlockStatePtrsToUserStatePtrs();
+    program_->SetParameterOffsetsAndIndex();
     return false;
   }
 
@@ -696,6 +699,8 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
     }
   }
 
+  program_->SetParameterBlockStatePtrsToUserStatePtrs();
+  program_->SetParameterOffsetsAndIndex();
   return status;
 }
 

internal/ceres/problem_test.cc

@@ -997,7 +997,9 @@ class ProblemEvaluateTest : public ::testing::Test {
                                   parameters_));
   }
 
-
+  void TearDown() {
+    EXPECT_TRUE(problem_.program().IsValid());
+  }
 
   void EvaluateAndCompare(const Problem::EvaluateOptions& options,
                           const int expected_num_rows,

internal/ceres/program.cc

@@ -140,6 +140,36 @@ void Program::SetParameterOffsetsAndIndex() {
   }
 }
 
+bool Program::IsValid() const {
+  for (int i = 0; i < residual_blocks_.size(); ++i) {
+    const ResidualBlock* residual_block = residual_blocks_[i];
+    if (residual_block->index() != i) {
+      LOG(WARNING) << "Residual block: " << i
+                   << " has incorrect index: " << residual_block->index();
+      return false;
+    }
+  }
+
+  int state_offset = 0;
+  int delta_offset = 0;
+  for (int i = 0; i < parameter_blocks_.size(); ++i) {
+    const ParameterBlock* parameter_block = parameter_blocks_[i];
+    if (parameter_block->index() != i ||
+        parameter_block->state_offset() != state_offset ||
+        parameter_block->delta_offset() != delta_offset) {
+      LOG(WARNING) << "Parameter block: " << i
+                   << "has incorrect indexing information: "
+                   << parameter_block->ToString();
+      return false;
+    }
+
+    state_offset += parameter_blocks_[i]->Size();
+    delta_offset += parameter_blocks_[i]->LocalSize();
+  }
+
+  return true;
+}
+
 int Program::NumResidualBlocks() const {
   return residual_blocks_.size();
 }

internal/ceres/program.h

@@ -99,6 +99,10 @@ class Program {
   // position of the parameter in the state and delta vector respectively.
   void SetParameterOffsetsAndIndex();
 
+  // Check if the internal state of the program (the indexing and the
+  // offsets) are correct.
+  bool IsValid() const;
+
   // See problem.h for what these do.
   int NumParameterBlocks() const;
   int NumParameters() const;

internal/ceres/solver_impl_test.cc

@@ -794,6 +794,8 @@ TEST(SolverImpl, ConstantParameterBlocksDoNotChangeAndStateInvariantKept) {
   EXPECT_EQ(&y, problem.program().parameter_blocks()[1]->state());
   EXPECT_EQ(&z, problem.program().parameter_blocks()[2]->state());
   EXPECT_EQ(&w, problem.program().parameter_blocks()[3]->state());
+
+  EXPECT_TRUE(problem.program().IsValid());
 }
 
 TEST(SolverImpl, NoParameterBlocks) {