diff --git a/cunls/minimizer/minimizer_state.cu b/cunls/minimizer/minimizer_state.cu index 45c782a..54e2f25 100644 --- a/cunls/minimizer/minimizer_state.cu +++ b/cunls/minimizer/minimizer_state.cu @@ -41,10 +41,9 @@ constexpr size_t block_size = 256; * @param num_states_in_batch Number of state elements in the batch. * @param num_new_pointers Number of pointers to remap. */ -__global__ void -set_state_pointers_kernel(float **new_pointers, float *const *old_pointers, - float *old_start_ptr, float *new_start_ptr, - size_t num_states_in_batch, size_t num_new_pointers) { +__global__ void set_state_pointers_kernel(float **new_pointers, float *const *old_pointers, + float *old_start_ptr, float *new_start_ptr, + size_t num_states_in_batch, size_t num_new_pointers) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= num_new_pointers) { return; @@ -69,7 +68,10 @@ set_state_pointers_kernel(float **new_pointers, float *const *old_pointers, */ void MinimizerState::CreateStates(const Problem &problem) { const auto &state_batches = problem.GetStateBatches(); - if (states_.size() < state_batches.size()) { + // Resize to the exact batch count so stale trailing batches from a previous, + // larger problem are dropped when the topology shrinks. Surviving batches keep + // their device buffers; only whole unused batch buffers are freed. + if (states_.size() != state_batches.size()) { states_.resize(state_batches.size()); } @@ -77,8 +79,7 @@ void MinimizerState::CreateStates(const Problem &problem) { const auto ¶m_batch_ptr = state_batches[i]; auto &state_vec = states_[i]; - size_t size = - param_batch_ptr->NumStateBlocks() * param_batch_ptr->AmbientSize(); + size_t size = param_batch_ptr->NumStateBlocks() * param_batch_ptr->AmbientSize(); if (state_vec.size() != size) { state_vec.resize(size); @@ -97,7 +98,7 @@ void MinimizerState::CreateStates(const Problem &problem) { */ void MinimizerState::CreateStatePointers(const Problem &problem) { const auto &problem_param_pointers = problem.GetStatePointers(); - if (state_pointers_.size() < problem_param_pointers.size()) { + if (state_pointers_.size() != problem_param_pointers.size()) { state_pointers_.resize(problem_param_pointers.size()); } @@ -111,14 +112,13 @@ void MinimizerState::CreateStatePointers(const Problem &problem) { void MinimizerState::CopyProblemStatePointersFromHost(const Problem &problem) { const auto &host = problem.GetStatePointers(); - if (problem_state_ptrs_device_.size() < host.size()) { + if (problem_state_ptrs_device_.size() != host.size()) { problem_state_ptrs_device_.resize(host.size()); } for (size_t i = 0; i < host.size(); ++i) { problem_state_ptrs_device_[i].resize(host[i].size()); if (!host[i].empty()) { - problem_state_ptrs_device_[i].CopyFromHost(host[i].data(), - host[i].size()); + problem_state_ptrs_device_[i].CopyFromHost(host[i].data(), host[i].size()); } } } @@ -149,8 +149,7 @@ void MinimizerState::Create(cudaStream_t stream, const Problem &problem) { auto &state_vec = states_[i]; float *ptr = param_batch_ptr->StateBlockDevicePtr(0); - size_t size = - param_batch_ptr->NumStateBlocks() * param_batch_ptr->AmbientSize(); + size_t size = param_batch_ptr->NumStateBlocks() * param_batch_ptr->AmbientSize(); thrust::device_ptr src_ptr(ptr); thrust::device_ptr dst_ptr(state_vec.data()); @@ -187,8 +186,8 @@ void MinimizerState::Create(cudaStream_t stream, const Problem &problem) { size_t num_blocks = (param_ptrs.size() + block_size - 1) / block_size; set_state_pointers_kernel<<>>( - new_pointers, old_pointers, state_batch_ptr, new_param_ptr, - num_states_in_batch, param_ptrs.size()); + new_pointers, old_pointers, state_batch_ptr, new_param_ptr, num_states_in_batch, + param_ptrs.size()); THROW_ON_CUDA_ERROR(cudaGetLastError()); } } @@ -204,8 +203,7 @@ void MinimizerState::Create(cudaStream_t stream, const Problem &problem) { * @param stream CUDA stream for GPU operations. * @param from Source state vectors to copy from. */ -void MinimizerState::Copy(cudaStream_t stream, - const std::vector> &from) { +void MinimizerState::Copy(cudaStream_t stream, const std::vector> &from) { if (states_.size() != from.size()) { states_.resize(from.size()); } @@ -251,4 +249,4 @@ void Copy(cudaStream_t stream, const MinimizerState &state, Problem &problem) { thrust::copy(stream_policy, src_ptr, src_ptr + dvec.size(), dst_ptr); } } -} // namespace cunls +} // namespace cunls diff --git a/tests/gauss_newton_test.cpp b/tests/gauss_newton_test.cpp index 7bb8d33..84cf968 100644 --- a/tests/gauss_newton_test.cpp +++ b/tests/gauss_newton_test.cpp @@ -50,7 +50,7 @@ namespace cunls { */ template class GaussNewtonMinimizerTest : public ::testing::Test { -public: + public: static constexpr int kDim = TestParam::vector_size; using StatesType = VectorStateBatch; using VectorType = Vector; @@ -76,16 +76,14 @@ class GaussNewtonMinimizerTest : public ::testing::Test { state_values_[i].fill(x); } - minimizer_options_.sparse_linear_solver_type = - SparseLinearSolverType::cuDSS; + minimizer_options_.sparse_linear_solver_type = SparseLinearSolverType::cuDSS; cuDSSLinearSolverOptions cudss_solver_options = { .mode = static_cast(TestParam::solver_id), .nthreads = 1, .threading_lib_path = "", }; - minimizer_options_.sparse_linear_solver_config = {.cudss_solver_options = - cudss_solver_options}; + minimizer_options_.sparse_linear_solver_config = {.cudss_solver_options = cudss_solver_options}; } /** @@ -97,23 +95,19 @@ class GaussNewtonMinimizerTest : public ::testing::Test { * * @param states Optimized state batch to verify. */ - void CheckConvergence(const StatesType &states, - const std::vector &const_state_ids = {}) { + void CheckConvergence(const StatesType &states, const std::vector &const_state_ids = {}) { size_t num_blocks = states.NumStateBlocks(); - auto ptr = - reinterpret_cast(states.StateBlockDevicePtr(0)); + auto ptr = reinterpret_cast(states.StateBlockDevicePtr(0)); std::vector host_states(num_blocks); - THROW_ON_CUDA_ERROR(cudaMemcpy(host_states.data(), ptr, - num_blocks * sizeof(VectorType), + THROW_ON_CUDA_ERROR(cudaMemcpy(host_states.data(), ptr, num_blocks * sizeof(VectorType), cudaMemcpyDeviceToHost)); ASSERT_EQ(host_states.size(), observations_.size()); for (size_t i = 0; i < num_blocks; i++) { const auto &obs = observations_[i]; const auto &state_vals = host_states[i]; - auto const_state_it = - std::find(const_state_ids.begin(), const_state_ids.end(), i); + auto const_state_it = std::find(const_state_ids.begin(), const_state_ids.end(), i); if (const_state_it != const_state_ids.end()) { // State is constant, verify it wasn't changed during optimization float x = static_cast(i); @@ -128,15 +122,14 @@ class GaussNewtonMinimizerTest : public ::testing::Test { } } - const size_t num_vectors_ = 10000; ///< Number of state blocks in test. + const size_t num_vectors_ = 10000; ///< Number of state blocks in test. - std::vector observations_; ///< Target values for optimization. - std::vector state_values_; ///< Initial state values. + std::vector observations_; ///< Target values for optimization. + std::vector state_values_; ///< Initial state values. MinimizerOptions minimizer_options_{.disable_safety_checks = false}; - profiler::Domain profiler_domain_{ - "GaussNewtonMinimizerTest"}; ///< Profiling domain. + profiler::Domain profiler_domain_{"GaussNewtonMinimizerTest"}; ///< Profiling domain. }; /** @@ -144,15 +137,15 @@ class GaussNewtonMinimizerTest : public ::testing::Test { * * @tparam Value Vector dimension (1, 2, 3, or 4). */ -template struct TestParam { +template +struct TestParam { static constexpr int vector_size = VectorSize; static constexpr int solver_id = SolverId; }; /** @brief Test types: 1D, 2D, 3D, and 4D vectors. */ -typedef ::testing::Types, TestParam<2, 0>, TestParam<3, 0>, - TestParam<4, 0>, TestParam<1, 1>, TestParam<2, 1>, - TestParam<3, 1>, TestParam<4, 1>> +typedef ::testing::Types, TestParam<2, 0>, TestParam<3, 0>, TestParam<4, 0>, + TestParam<1, 1>, TestParam<2, 1>, TestParam<3, 1>, TestParam<4, 1>> TestParams; TYPED_TEST_CASE(GaussNewtonMinimizerTest, TestParams); @@ -193,11 +186,9 @@ TYPED_TEST(GaussNewtonMinimizerTest, SimpleGN) { * states. */ TYPED_TEST(GaussNewtonMinimizerTest, GNWithConstantStates) { - auto test_range = - this->profiler_domain_.CreateDomainRange("GNWithConstantStates"); + auto test_range = this->profiler_domain_.CreateDomainRange("GNWithConstantStates"); std::vector const_state_ids = {0, 9, 99, 999}; - typename TestFixture::StateData state_data(this->state_values_, - const_state_ids); + typename TestFixture::StateData state_data(this->state_values_, const_state_ids); auto &vector_states = state_data.get(); auto device_pointers = test_utils::CollectStatePointers(vector_states); typename TestFixture::FactorData factor_data(this->observations_); @@ -253,11 +244,9 @@ TYPED_TEST(GaussNewtonMinimizerTest, SimpleLM) { * during optimization while still optimizing other states. */ TYPED_TEST(GaussNewtonMinimizerTest, LMWithConstantStates) { - auto test_range = - this->profiler_domain_.CreateDomainRange("LMWithConstantStates"); + auto test_range = this->profiler_domain_.CreateDomainRange("LMWithConstantStates"); std::vector const_state_ids = {0, 9, 99, 999}; - typename TestFixture::StateData state_data(this->state_values_, - const_state_ids); + typename TestFixture::StateData state_data(this->state_values_, const_state_ids); auto &vector_states = state_data.get(); auto device_pointers = test_utils::CollectStatePointers(vector_states); typename TestFixture::FactorData factor_data(this->observations_); @@ -282,8 +271,7 @@ TYPED_TEST(GaussNewtonMinimizerTest, LMWithConstantStates) { * @brief Levenberg-Marquardt with Hessian-diagonal column scaling. */ TYPED_TEST(GaussNewtonMinimizerTest, LMColumnScalingHessianDiagonal) { - auto test_range = this->profiler_domain_.CreateDomainRange( - "LMColumnScalingHessianDiagonal"); + auto test_range = this->profiler_domain_.CreateDomainRange("LMColumnScalingHessianDiagonal"); typename TestFixture::StateData state_data(this->state_values_); auto &vector_states = state_data.get(); auto device_pointers = test_utils::CollectStatePointers(vector_states); @@ -309,8 +297,7 @@ TYPED_TEST(GaussNewtonMinimizerTest, LMColumnScalingHessianDiagonal) { * @brief Levenberg-Marquardt with Jacobian column-norm scaling. */ TYPED_TEST(GaussNewtonMinimizerTest, LMColumnScalingJacobianColumnNorm) { - auto test_range = this->profiler_domain_.CreateDomainRange( - "LMColumnScalingJacobianColumnNorm"); + auto test_range = this->profiler_domain_.CreateDomainRange("LMColumnScalingJacobianColumnNorm"); typename TestFixture::StateData state_data(this->state_values_); auto &vector_states = state_data.get(); auto device_pointers = test_utils::CollectStatePointers(vector_states); @@ -336,8 +323,7 @@ TYPED_TEST(GaussNewtonMinimizerTest, LMColumnScalingJacobianColumnNorm) { * @brief Gauss-Newton with column scaling (shared MinimizerOptions path). */ TYPED_TEST(GaussNewtonMinimizerTest, GNColumnScalingHessianDiagonal) { - auto test_range = this->profiler_domain_.CreateDomainRange( - "GNColumnScalingHessianDiagonal"); + auto test_range = this->profiler_domain_.CreateDomainRange("GNColumnScalingHessianDiagonal"); typename TestFixture::StateData state_data(this->state_values_); auto &vector_states = state_data.get(); auto device_pointers = test_utils::CollectStatePointers(vector_states); @@ -389,8 +375,7 @@ TEST(MinimizeBufferReuse, GaussNewtonTwiceIdenticalSummaries) { .nthreads = 1, .threading_lib_path = "", }; - opts.sparse_linear_solver_config = {.cudss_solver_options = - cudss_solver_options}; + opts.sparse_linear_solver_config = {.cudss_solver_options = cudss_solver_options}; opts.disable_safety_checks = false; CudaStream stream; @@ -399,15 +384,13 @@ TEST(MinimizeBufferReuse, GaussNewtonTwiceIdenticalSummaries) { float *state_base = vector_states.StateBlockDevicePtr(0); const size_t num_floats = n * 1; std::vector initial_host(num_floats); - THROW_ON_CUDA_ERROR(cudaMemcpy(initial_host.data(), state_base, - num_floats * sizeof(float), + THROW_ON_CUDA_ERROR(cudaMemcpy(initial_host.data(), state_base, num_floats * sizeof(float), cudaMemcpyDeviceToHost)); MinimizerSummary s1 = minimizer.Minimize(stream.GetStream(), problem); THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); - THROW_ON_CUDA_ERROR(cudaMemcpy(state_base, initial_host.data(), - num_floats * sizeof(float), + THROW_ON_CUDA_ERROR(cudaMemcpy(state_base, initial_host.data(), num_floats * sizeof(float), cudaMemcpyHostToDevice)); MinimizerSummary s2 = minimizer.Minimize(stream.GetStream(), problem); @@ -418,4 +401,66 @@ TEST(MinimizeBufferReuse, GaussNewtonTwiceIdenticalSummaries) { EXPECT_NEAR(s1.initial_cost, s2.initial_cost, 1e-4f); } -} // namespace cunls +/** + * @brief Reusing a minimizer after the number of state batches decreases must + * not retain stale state vectors from the previous problem. + * + * Regression test for a two-batch to one-batch topology transition. Before the + * fix, MinimizerState::CreateStates only grew its outer vector. UpdateStates + * consequently passed two state pointers to StateBatchOps configured for one + * batch, causing an assertion failure in debug builds and an out-of-bounds + * access in release builds. + */ +TEST(MinimizeBufferReuse, GaussNewtonStateBatchCountDecreases) { + constexpr size_t n = 32; + const auto observations = test_utils::MakeZeroVectors<1>(n); + const auto initial_states = test_utils::MakeConstantVectors<1>(n, 1.0f); + + MinimizerOptions opts; + opts.sparse_linear_solver_type = SparseLinearSolverType::cuDSS; + cuDSSLinearSolverOptions cudss_solver_options = { + .mode = cuDSSLinearSolverMode::SlowInitFastSolve, + .nthreads = 1, + .threading_lib_path = "", + }; + opts.sparse_linear_solver_config = {.cudss_solver_options = cudss_solver_options}; + opts.disable_safety_checks = false; + + CudaStream stream; + GaussNewtonMinimizer minimizer(opts); + + { + test_utils::VectorStateData<1> first_state_data(initial_states); + test_utils::VectorStateData<1> second_state_data(initial_states); + test_utils::PriorFactorData<1> first_factor_data(observations); + test_utils::PriorFactorData<1> second_factor_data(observations); + + Problem two_batch_problem; + two_batch_problem.AddStateBatch(first_state_data.ptr()); + two_batch_problem.AddStateBatch(second_state_data.ptr()); + two_batch_problem.AddFactorBatch(&first_factor_data.get(), + test_utils::CollectStatePointers(first_state_data.get())); + two_batch_problem.AddFactorBatch(&second_factor_data.get(), + test_utils::CollectStatePointers(second_state_data.get())); + ASSERT_TRUE(two_batch_problem.CheckConsistency()); + + minimizer.Minimize(stream.GetStream(), two_batch_problem); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + } + + { + test_utils::VectorStateData<1> state_data(initial_states); + test_utils::PriorFactorData<1> factor_data(observations); + + Problem one_batch_problem; + one_batch_problem.AddStateBatch(state_data.ptr()); + one_batch_problem.AddFactorBatch(&factor_data.get(), + test_utils::CollectStatePointers(state_data.get())); + ASSERT_TRUE(one_batch_problem.CheckConsistency()); + + EXPECT_NO_THROW(minimizer.Minimize(stream.GetStream(), one_batch_problem)); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + } +} + +} // namespace cunls