diff options
Diffstat (limited to 'internal/ceres/problem_test.cc')
-rw-r--r-- | internal/ceres/problem_test.cc | 286 |
1 files changed, 253 insertions, 33 deletions
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc index 0944d3f..db082ec 100644 --- a/internal/ceres/problem_test.cc +++ b/internal/ceres/problem_test.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2013 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -56,7 +56,7 @@ namespace internal { // Trivial cost function that accepts a single argument. class UnaryCostFunction : public CostFunction { public: - UnaryCostFunction(int num_residuals, int16 parameter_block_size) { + UnaryCostFunction(int num_residuals, int32 parameter_block_size) { set_num_residuals(num_residuals); mutable_parameter_block_sizes()->push_back(parameter_block_size); } @@ -76,8 +76,8 @@ class UnaryCostFunction : public CostFunction { class BinaryCostFunction: public CostFunction { public: BinaryCostFunction(int num_residuals, - int16 parameter_block1_size, - int16 parameter_block2_size) { + int32 parameter_block1_size, + int32 parameter_block2_size) { set_num_residuals(num_residuals); mutable_parameter_block_sizes()->push_back(parameter_block1_size); mutable_parameter_block_sizes()->push_back(parameter_block2_size); @@ -97,9 +97,9 @@ class BinaryCostFunction: public CostFunction { class TernaryCostFunction: public CostFunction { public: TernaryCostFunction(int num_residuals, - int16 parameter_block1_size, - int16 parameter_block2_size, - int16 parameter_block3_size) { + int32 parameter_block1_size, + int32 parameter_block2_size, + int32 parameter_block3_size) { set_num_residuals(num_residuals); mutable_parameter_block_sizes()->push_back(parameter_block1_size); mutable_parameter_block_sizes()->push_back(parameter_block2_size); @@ -139,7 +139,7 @@ TEST(Problem, AddResidualWithIncorrectNumberOfParameterBlocksDies) { // UnaryCostFunction takes only one parameter, but two are passed. EXPECT_DEATH_IF_SUPPORTED( problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x, y), - "parameter_blocks.size()"); + "parameter_blocks.size"); } TEST(Problem, AddResidualWithDifferentSizesOnTheSameVariableDies) { @@ -378,7 +378,7 @@ TEST(Problem, CostFunctionsAreDeletedEvenWithRemovals) { struct DynamicProblem : public ::testing::TestWithParam<bool> { DynamicProblem() { Problem::Options options; - options.enable_fast_parameter_block_removal = GetParam(); + options.enable_fast_removal = GetParam(); problem.reset(new ProblemImpl(options)); } @@ -390,9 +390,26 @@ struct DynamicProblem : public ::testing::TestWithParam<bool> { } bool HasResidualBlock(ResidualBlock* residual_block) { - return find(problem->program().residual_blocks().begin(), - problem->program().residual_blocks().end(), - residual_block) != problem->program().residual_blocks().end(); + bool have_residual_block = true; + if (GetParam()) { + have_residual_block &= + (problem->residual_block_set().find(residual_block) != + problem->residual_block_set().end()); + } + have_residual_block &= + find(problem->program().residual_blocks().begin(), + problem->program().residual_blocks().end(), + residual_block) != problem->program().residual_blocks().end(); + return have_residual_block; + } + + int NumResidualBlocks() { + // Verify that the hash set of residuals is maintained consistently. + if (GetParam()) { + EXPECT_EQ(problem->residual_block_set().size(), + problem->NumResidualBlocks()); + } + return problem->NumResidualBlocks(); } // The next block of functions until the end are only for testing the @@ -502,6 +519,20 @@ TEST(Problem, RemoveParameterBlockWithUnknownPtrDies) { problem.RemoveParameterBlock(y), "Parameter block not found:"); } +TEST(Problem, GetParameterization) { + double x[3]; + double y[2]; + + Problem problem; + problem.AddParameterBlock(x, 3); + problem.AddParameterBlock(y, 2); + + LocalParameterization* parameterization = new IdentityParameterization(3); + problem.SetParameterization(x, parameterization); + EXPECT_EQ(problem.GetParameterization(x), parameterization); + EXPECT_TRUE(problem.GetParameterization(y) == NULL); +} + TEST(Problem, ParameterBlockQueryTest) { double x[3]; double y[4]; @@ -525,7 +556,9 @@ TEST(Problem, ParameterBlockQueryTest) { EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y); EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y); + EXPECT_TRUE(problem.HasParameterBlock(x)); problem.RemoveParameterBlock(x); + EXPECT_FALSE(problem.HasParameterBlock(x)); problem.GetParameterBlocks(¶meter_blocks); EXPECT_EQ(parameter_blocks.size(), 1); EXPECT_TRUE(parameter_blocks[0] == y); @@ -536,7 +569,7 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { problem->AddParameterBlock(z, 5); problem->AddParameterBlock(w, 3); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(y, GetParameterBlock(0)->user_state()); EXPECT_EQ(z, GetParameterBlock(1)->user_state()); EXPECT_EQ(w, GetParameterBlock(2)->user_state()); @@ -545,12 +578,12 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { // removing it. problem->RemoveParameterBlock(w); ASSERT_EQ(2, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(y, GetParameterBlock(0)->user_state()); EXPECT_EQ(z, GetParameterBlock(1)->user_state()); problem->AddParameterBlock(w, 3); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(y, GetParameterBlock(0)->user_state()); EXPECT_EQ(z, GetParameterBlock(1)->user_state()); EXPECT_EQ(w, GetParameterBlock(2)->user_state()); @@ -558,12 +591,12 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { // Now remove z, which is in the middle, and add it back. problem->RemoveParameterBlock(z); ASSERT_EQ(2, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(y, GetParameterBlock(0)->user_state()); EXPECT_EQ(w, GetParameterBlock(1)->user_state()); problem->AddParameterBlock(z, 5); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(y, GetParameterBlock(0)->user_state()); EXPECT_EQ(w, GetParameterBlock(1)->user_state()); EXPECT_EQ(z, GetParameterBlock(2)->user_state()); @@ -572,20 +605,20 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { // y problem->RemoveParameterBlock(y); ASSERT_EQ(2, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(z, GetParameterBlock(0)->user_state()); EXPECT_EQ(w, GetParameterBlock(1)->user_state()); // z problem->RemoveParameterBlock(z); ASSERT_EQ(1, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(w, GetParameterBlock(0)->user_state()); // w problem->RemoveParameterBlock(w); EXPECT_EQ(0, problem->NumParameterBlocks()); - EXPECT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(0, NumResidualBlocks()); } TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { @@ -593,7 +626,7 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { problem->AddParameterBlock(z, 5); problem->AddParameterBlock(w, 3); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); EXPECT_EQ(y, GetParameterBlock(0)->user_state()); EXPECT_EQ(z, GetParameterBlock(1)->user_state()); EXPECT_EQ(w, GetParameterBlock(2)->user_state()); @@ -616,12 +649,12 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); EXPECT_EQ(3, problem->NumParameterBlocks()); - EXPECT_EQ(7, problem->NumResidualBlocks()); + EXPECT_EQ(7, NumResidualBlocks()); // Remove w, which should remove r_yzw, r_yw, r_zw, r_w. problem->RemoveParameterBlock(w); ASSERT_EQ(2, problem->NumParameterBlocks()); - ASSERT_EQ(3, problem->NumResidualBlocks()); + ASSERT_EQ(3, NumResidualBlocks()); ASSERT_FALSE(HasResidualBlock(r_yzw)); ASSERT_TRUE (HasResidualBlock(r_yz )); @@ -634,7 +667,7 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { // Remove z, which will remove almost everything else. problem->RemoveParameterBlock(z); ASSERT_EQ(1, problem->NumParameterBlocks()); - ASSERT_EQ(1, problem->NumResidualBlocks()); + ASSERT_EQ(1, NumResidualBlocks()); ASSERT_FALSE(HasResidualBlock(r_yzw)); ASSERT_FALSE(HasResidualBlock(r_yz )); @@ -647,7 +680,7 @@ TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { // Remove y; all gone. problem->RemoveParameterBlock(y); EXPECT_EQ(0, problem->NumParameterBlocks()); - EXPECT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(0, NumResidualBlocks()); } TEST_P(DynamicProblem, RemoveResidualBlock) { @@ -685,14 +718,14 @@ TEST_P(DynamicProblem, RemoveResidualBlock) { EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL); } EXPECT_EQ(3, problem->NumParameterBlocks()); - EXPECT_EQ(7, problem->NumResidualBlocks()); + EXPECT_EQ(7, NumResidualBlocks()); // Remove each residual and check the state after each removal. // Remove r_yzw. problem->RemoveResidualBlock(r_yzw); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(6, problem->NumResidualBlocks()); + ASSERT_EQ(6, NumResidualBlocks()); if (GetParam()) { ExpectParameterBlockContains(y, r_yz, r_yw, r_y); ExpectParameterBlockContains(z, r_yz, r_zw, r_z); @@ -708,7 +741,7 @@ TEST_P(DynamicProblem, RemoveResidualBlock) { // Remove r_yw. problem->RemoveResidualBlock(r_yw); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(5, problem->NumResidualBlocks()); + ASSERT_EQ(5, NumResidualBlocks()); if (GetParam()) { ExpectParameterBlockContains(y, r_yz, r_y); ExpectParameterBlockContains(z, r_yz, r_zw, r_z); @@ -723,7 +756,7 @@ TEST_P(DynamicProblem, RemoveResidualBlock) { // Remove r_zw. problem->RemoveResidualBlock(r_zw); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(4, problem->NumResidualBlocks()); + ASSERT_EQ(4, NumResidualBlocks()); if (GetParam()) { ExpectParameterBlockContains(y, r_yz, r_y); ExpectParameterBlockContains(z, r_yz, r_z); @@ -737,7 +770,7 @@ TEST_P(DynamicProblem, RemoveResidualBlock) { // Remove r_w. problem->RemoveResidualBlock(r_w); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(3, problem->NumResidualBlocks()); + ASSERT_EQ(3, NumResidualBlocks()); if (GetParam()) { ExpectParameterBlockContains(y, r_yz, r_y); ExpectParameterBlockContains(z, r_yz, r_z); @@ -750,7 +783,7 @@ TEST_P(DynamicProblem, RemoveResidualBlock) { // Remove r_yz. problem->RemoveResidualBlock(r_yz); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(2, problem->NumResidualBlocks()); + ASSERT_EQ(2, NumResidualBlocks()); if (GetParam()) { ExpectParameterBlockContains(y, r_y); ExpectParameterBlockContains(z, r_z); @@ -763,7 +796,7 @@ TEST_P(DynamicProblem, RemoveResidualBlock) { problem->RemoveResidualBlock(r_z); problem->RemoveResidualBlock(r_y); ASSERT_EQ(3, problem->NumParameterBlocks()); - ASSERT_EQ(0, problem->NumResidualBlocks()); + ASSERT_EQ(0, NumResidualBlocks()); if (GetParam()) { ExpectParameterBlockContains(y); ExpectParameterBlockContains(z); @@ -771,6 +804,191 @@ TEST_P(DynamicProblem, RemoveResidualBlock) { } } +TEST_P(DynamicProblem, RemoveInvalidResidualBlockDies) { + problem->AddParameterBlock(y, 4); + problem->AddParameterBlock(z, 5); + problem->AddParameterBlock(w, 3); + + // Add all combinations of cost functions. + CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); + CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); + CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); + CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); + CostFunction* cost_y = new UnaryCostFunction (1, 4); + CostFunction* cost_z = new UnaryCostFunction (1, 5); + CostFunction* cost_w = new UnaryCostFunction (1, 3); + + ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); + ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); + ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); + ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); + ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); + ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); + ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); + + // Remove r_yzw. + problem->RemoveResidualBlock(r_yzw); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(6, NumResidualBlocks()); + // Attempt to remove r_yzw again. + EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yzw), "not found"); + + // Attempt to remove a cast pointer never added as a residual. + int trash_memory = 1234; + ResidualBlock* invalid_residual = + reinterpret_cast<ResidualBlock*>(&trash_memory); + EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(invalid_residual), + "not found"); + + // Remove a parameter block, which in turn removes the dependent residuals + // then attempt to remove them directly. + problem->RemoveParameterBlock(z); + ASSERT_EQ(2, problem->NumParameterBlocks()); + ASSERT_EQ(3, NumResidualBlocks()); + EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yz), "not found"); + EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_zw), "not found"); + EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_z), "not found"); + + problem->RemoveResidualBlock(r_yw); + problem->RemoveResidualBlock(r_w); + problem->RemoveResidualBlock(r_y); +} + +// Check that a null-terminated array, a, has the same elements as b. +template<typename T> +void ExpectVectorContainsUnordered(const T* a, const vector<T>& b) { + // Compute the size of a. + int size = 0; + while (a[size]) { + ++size; + } + ASSERT_EQ(size, b.size()); + + // Sort a. + vector<T> a_sorted(size); + copy(a, a + size, a_sorted.begin()); + sort(a_sorted.begin(), a_sorted.end()); + + // Sort b. + vector<T> b_sorted(b); + sort(b_sorted.begin(), b_sorted.end()); + + // Compare. + for (int i = 0; i < size; ++i) { + EXPECT_EQ(a_sorted[i], b_sorted[i]); + } +} + +void ExpectProblemHasResidualBlocks( + const ProblemImpl &problem, + const ResidualBlockId *expected_residual_blocks) { + vector<ResidualBlockId> residual_blocks; + problem.GetResidualBlocks(&residual_blocks); + ExpectVectorContainsUnordered(expected_residual_blocks, residual_blocks); +} + +TEST_P(DynamicProblem, GetXXXBlocksForYYYBlock) { + problem->AddParameterBlock(y, 4); + problem->AddParameterBlock(z, 5); + problem->AddParameterBlock(w, 3); + + // Add all combinations of cost functions. + CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); + CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); + CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); + CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); + CostFunction* cost_y = new UnaryCostFunction (1, 4); + CostFunction* cost_z = new UnaryCostFunction (1, 5); + CostFunction* cost_w = new UnaryCostFunction (1, 3); + + ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); + { + ResidualBlockId expected_residuals[] = {r_yzw, 0}; + ExpectProblemHasResidualBlocks(*problem, expected_residuals); + } + ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); + { + ResidualBlockId expected_residuals[] = {r_yzw, r_yz, 0}; + ExpectProblemHasResidualBlocks(*problem, expected_residuals); + } + ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); + { + ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, 0}; + ExpectProblemHasResidualBlocks(*problem, expected_residuals); + } + ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); + { + ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, 0}; + ExpectProblemHasResidualBlocks(*problem, expected_residuals); + } + ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); + { + ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, r_y, 0}; + ExpectProblemHasResidualBlocks(*problem, expected_residuals); + } + ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); + { + ResidualBlock *expected_residuals[] = { + r_yzw, r_yz, r_yw, r_zw, r_y, r_z, 0 + }; + ExpectProblemHasResidualBlocks(*problem, expected_residuals); + } + ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); + { + ResidualBlock *expected_residuals[] = { + r_yzw, r_yz, r_yw, r_zw, r_y, r_z, r_w, 0 + }; + ExpectProblemHasResidualBlocks(*problem, expected_residuals); + } + + vector<double*> parameter_blocks; + vector<ResidualBlockId> residual_blocks; + + // Check GetResidualBlocksForParameterBlock() for all parameter blocks. + struct GetResidualBlocksForParameterBlockTestCase { + double* parameter_block; + ResidualBlockId expected_residual_blocks[10]; + }; + GetResidualBlocksForParameterBlockTestCase get_residual_blocks_cases[] = { + { y, { r_yzw, r_yz, r_yw, r_y, NULL} }, + { z, { r_yzw, r_yz, r_zw, r_z, NULL} }, + { w, { r_yzw, r_yw, r_zw, r_w, NULL} }, + { NULL } + }; + for (int i = 0; get_residual_blocks_cases[i].parameter_block; ++i) { + problem->GetResidualBlocksForParameterBlock( + get_residual_blocks_cases[i].parameter_block, + &residual_blocks); + ExpectVectorContainsUnordered( + get_residual_blocks_cases[i].expected_residual_blocks, + residual_blocks); + } + + // Check GetParameterBlocksForResidualBlock() for all residual blocks. + struct GetParameterBlocksForResidualBlockTestCase { + ResidualBlockId residual_block; + double* expected_parameter_blocks[10]; + }; + GetParameterBlocksForResidualBlockTestCase get_parameter_blocks_cases[] = { + { r_yzw, { y, z, w, NULL } }, + { r_yz , { y, z, NULL } }, + { r_yw , { y, w, NULL } }, + { r_zw , { z, w, NULL } }, + { r_y , { y, NULL } }, + { r_z , { z, NULL } }, + { r_w , { w, NULL } }, + { NULL } + }; + for (int i = 0; get_parameter_blocks_cases[i].residual_block; ++i) { + problem->GetParameterBlocksForResidualBlock( + get_parameter_blocks_cases[i].residual_block, + ¶meter_blocks); + ExpectVectorContainsUnordered( + get_parameter_blocks_cases[i].expected_parameter_blocks, + parameter_blocks); + } +} + INSTANTIATE_TEST_CASE_P(OptionsInstantiation, DynamicProblem, ::testing::Values(true, false)); @@ -862,7 +1080,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, |