Fix Problem::RemoveParameterBlock after calling solve.

The indexing parameter in the parameter block is expected
to be the same as its position in the program. However,
when Solve is called, the parameter blocks are made
members of a new program and the same indexing parameter
is used to store the position of the parameter block
in this new reordered program.

So when the user calls RemoveParameterBlock after calling
Solve, and ProblemImpl checks the invariant on the
indexing parameter it does not match and causes a crash.

The fix is to make sure that before returning from Solve,
the state of the indexing parameter's values are restored
to their original values corresponding to their positions
in the Program object contained in the ProblemImpl object.

Thanks to Simon Lynen for reporting this.

Change-Id: I060745026cd23a688c1fb5e2c9e6053e9ba1c78f

internal/ceres/problem_impl.cc

diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index 8302702..0846de8 100644
--- a/internal/ceres/problem_impl.cc
+++ b/internal/ceres/problem_impl.cc
@@ -452,7 +452,11 @@
 void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks,
                                       Block* block_to_remove) {
   CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove)
-      << "You found a Ceres bug! Block: " << block_to_remove->ToString();
+      << "You found a Ceres bug! \n"
+      << "Block requested: "
+      << block_to_remove->ToString() << "\n"
+      << "Block present: "
+      << (*mutable_blocks)[block_to_remove->index()]->ToString();
 
   // Prepare the to-be-moved block for the new, lower-in-index position by
   // setting the index to the blocks final location.

internal/ceres/solver_impl.cc

diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 83faa05..ae7fc3f 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -494,6 +494,7 @@
 
     // Ensure the program state is set to the user parameters on the way out.
     original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+    original_program->SetParameterOffsetsAndIndex();
 
     summary->postprocessor_time_in_seconds =
         WallTimeInSeconds() - post_process_start_time;
@@ -596,6 +597,7 @@
   // Ensure the program state is set to the user parameters on the way
   // out.
   original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+  original_program->SetParameterOffsetsAndIndex();
 
   const map<string, double>& linear_solver_time_statistics =
       linear_solver->TimeStatistics();
@@ -839,6 +841,8 @@
 
     // Ensure the program state is set to the user parameters on the way out.
     original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+    original_program->SetParameterOffsetsAndIndex();
+
     summary->postprocessor_time_in_seconds =
         WallTimeInSeconds() - post_process_start_time;
     return;
@@ -889,6 +893,7 @@
 
   // Ensure the program state is set to the user parameters on the way out.
   original_program->SetParameterBlockStatePtrsToUserStatePtrs();
+  original_program->SetParameterOffsetsAndIndex();
 
   const map<string, double>& evaluator_time_statistics =
       evaluator->TimeStatistics();