Don't assume program state is user state

The SolverImpl::Solve() method incorrectly assumed that the
state pointers inside the parameter blocks always pointed to
the user state at the start of the method. That is not true.

Change-Id: I73f8eeda453422c99e09d71a3cd0bfa92dd45742

internal/ceres/program.cc

diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc
index 444b102..529e4a3 100644
--- a/internal/ceres/program.cc
+++ b/internal/ceres/program.cc
@@ -86,11 +86,19 @@
 
 void Program::CopyParameterBlockStateToUserState() {
   for (int i = 0; i < parameter_blocks_.size(); ++i) {
-    parameter_blocks_[i]->GetState(
-        parameter_blocks_[i]->mutable_user_state());
+    parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state());
   }
 }
 
+bool Program::CopyUserStateToParameterBlocks() {
+  for (int i = 0; i < parameter_blocks_.size(); ++i) {
+    if (!parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
+      return false;
+    }
+  }
+  return true;
+}
+
 bool Program::Plus(const double* state,
                    const double* delta,
                    double* state_plus_delta) const {

internal/ceres/program.h

diff --git a/internal/ceres/program.h b/internal/ceres/program.h
index 113d352..27b58e1 100644
--- a/internal/ceres/program.h
+++ b/internal/ceres/program.h
@@ -71,8 +71,9 @@
   bool StateVectorToParameterBlocks(const double *state);
   void ParameterBlocksToStateVector(double *state) const;
 
-  // Copy internal state out to the user's parameters.
+  // Copy internal state to and from the user's parameters.
   void CopyParameterBlockStateToUserState();
+  bool CopyUserStateToParameterBlocks();
 
   // Update a state vector for the program given a delta.
   bool Plus(const double* state,

internal/ceres/solver_impl.cc

diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index ca35adc..506ef2f 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -207,6 +207,16 @@
       options.sparse_linear_algebra_library;
   summary->trust_region_strategy_type = options.trust_region_strategy_type;
 
+  // Ensure the program state is set to the user parameters.
+  Program* program = CHECK_NOTNULL(problem_impl)->mutable_program();
+  if (!program->CopyUserStateToParameterBlocks())  {
+    // This can only happen if there was a numerical problem updating the local
+    // jacobians. Indicate as such and fail out.
+    summary->termination_type == NUMERICAL_FAILURE;
+    summary->error = "Local parameterization failure.";
+    return;
+  }
+
   // Evaluate the initial cost and residual vector (if needed). The
   // initial cost needs to be computed on the original unpreprocessed
   // problem, as it is used to determine the value of the "fixed" part
@@ -315,7 +325,6 @@
   time_t post_process_end_time = time(NULL);
   summary->postprocessor_time_in_seconds =
       post_process_end_time - post_process_start_time;
-  return;
 }
 
 // Strips varying parameters and residuals, maintaining order, and updating