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