diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc index ae87fcb03..09eed9e61 100644 --- a/internal/ceres/problem_impl.cc +++ b/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; } diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc index a7f4f0bd9..3d0ea8206 100644 --- a/internal/ceres/problem_test.cc +++ b/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, diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc index 82d76d392..9e5c51bd6 100644 --- a/internal/ceres/program.cc +++ b/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(); } diff --git a/internal/ceres/program.h b/internal/ceres/program.h index 5002b7e75..4288f609c 100644 --- a/internal/ceres/program.h +++ b/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; diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc index f0f36add4..a605de06c 100644 --- a/internal/ceres/solver_impl_test.cc +++ b/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) {