| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. |
| // http://code.google.com/p/ceres-solver/ |
| // |
| // Redistribution and use in source and binary forms, with or without |
| // modification, are permitted provided that the following conditions are met: |
| // |
| // * Redistributions of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // * Redistributions in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // * Neither the name of Google Inc. nor the names of its contributors may be |
| // used to endorse or promote products derived from this software without |
| // specific prior written permission. |
| // |
| // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // POSSIBILITY OF SUCH DAMAGE. |
| // |
| // Author: sameeragarwal@google.com (Sameer Agarwal) |
| // keir@google.com (Keir Mierle) |
| |
| #include "ceres/problem.h" |
| #include "ceres/problem_impl.h" |
| |
| #include "gtest/gtest.h" |
| #include "ceres/cost_function.h" |
| #include "ceres/local_parameterization.h" |
| #include "ceres/map_util.h" |
| #include "ceres/parameter_block.h" |
| #include "ceres/program.h" |
| #include "ceres/sized_cost_function.h" |
| #include "ceres/internal/scoped_ptr.h" |
| |
| namespace ceres { |
| namespace internal { |
| |
| // The following three classes are for the purposes of defining |
| // function signatures. They have dummy Evaluate functions. |
| |
| // Trivial cost function that accepts a single argument. |
| class UnaryCostFunction : public CostFunction { |
| public: |
| UnaryCostFunction(int num_residuals, int16 parameter_block_size) { |
| set_num_residuals(num_residuals); |
| mutable_parameter_block_sizes()->push_back(parameter_block_size); |
| } |
| virtual ~UnaryCostFunction() {} |
| |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| for (int i = 0; i < num_residuals(); ++i) { |
| residuals[i] = 1; |
| } |
| return true; |
| } |
| }; |
| |
| // Trivial cost function that accepts two arguments. |
| class BinaryCostFunction: public CostFunction { |
| public: |
| BinaryCostFunction(int num_residuals, |
| int16 parameter_block1_size, |
| int16 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); |
| } |
| |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| for (int i = 0; i < num_residuals(); ++i) { |
| residuals[i] = 2; |
| } |
| return true; |
| } |
| }; |
| |
| // Trivial cost function that accepts three arguments. |
| class TernaryCostFunction: public CostFunction { |
| public: |
| TernaryCostFunction(int num_residuals, |
| int16 parameter_block1_size, |
| int16 parameter_block2_size, |
| int16 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); |
| mutable_parameter_block_sizes()->push_back(parameter_block3_size); |
| } |
| |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| for (int i = 0; i < num_residuals(); ++i) { |
| residuals[i] = 3; |
| } |
| return true; |
| } |
| }; |
| |
| TEST(Problem, AddResidualWithNullCostFunctionDies) { |
| double x[3], y[4], z[5]; |
| |
| Problem problem; |
| problem.AddParameterBlock(x, 3); |
| problem.AddParameterBlock(y, 4); |
| problem.AddParameterBlock(z, 5); |
| |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(NULL, NULL, x), |
| "'cost_function' Must be non NULL"); |
| } |
| |
| TEST(Problem, AddResidualWithIncorrectNumberOfParameterBlocksDies) { |
| double x[3], y[4], z[5]; |
| |
| Problem problem; |
| problem.AddParameterBlock(x, 3); |
| problem.AddParameterBlock(y, 4); |
| problem.AddParameterBlock(z, 5); |
| |
| // 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()"); |
| } |
| |
| TEST(Problem, AddResidualWithDifferentSizesOnTheSameVariableDies) { |
| double x[3]; |
| |
| Problem problem; |
| problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( |
| new UnaryCostFunction( |
| 2, 4 /* 4 != 3 */), NULL, x), |
| "different block sizes"); |
| } |
| |
| TEST(Problem, AddResidualWithDuplicateParametersDies) { |
| double x[3], z[5]; |
| |
| Problem problem; |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( |
| new BinaryCostFunction(2, 3, 3), NULL, x, x), |
| "Duplicate parameter blocks"); |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( |
| new TernaryCostFunction(1, 5, 3, 5), |
| NULL, z, x, z), |
| "Duplicate parameter blocks"); |
| } |
| |
| TEST(Problem, AddResidualWithIncorrectSizesOfParameterBlockDies) { |
| double x[3], y[4], z[5]; |
| |
| Problem problem; |
| problem.AddParameterBlock(x, 3); |
| problem.AddParameterBlock(y, 4); |
| problem.AddParameterBlock(z, 5); |
| |
| // The cost function expects the size of the second parameter, z, to be 4 |
| // instead of 5 as declared above. This is fatal. |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( |
| new BinaryCostFunction(2, 3, 4), NULL, x, z), |
| "different block sizes"); |
| } |
| |
| TEST(Problem, AddResidualAddsDuplicatedParametersOnlyOnce) { |
| double x[3], y[4], z[5]; |
| |
| Problem problem; |
| problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); |
| problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); |
| problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y); |
| problem.AddResidualBlock(new UnaryCostFunction(2, 5), NULL, z); |
| |
| EXPECT_EQ(3, problem.NumParameterBlocks()); |
| EXPECT_EQ(12, problem.NumParameters()); |
| } |
| |
| TEST(Problem, AddParameterWithDifferentSizesOnTheSameVariableDies) { |
| double x[3], y[4]; |
| |
| Problem problem; |
| problem.AddParameterBlock(x, 3); |
| problem.AddParameterBlock(y, 4); |
| |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(x, 4), |
| "different block sizes"); |
| } |
| |
| static double *IntToPtr(int i) { |
| return reinterpret_cast<double*>(sizeof(double) * i); // NOLINT |
| } |
| |
| TEST(Problem, AddParameterWithAliasedParametersDies) { |
| // Layout is |
| // |
| // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
| // [x] x x x x [y] y y |
| // o==o==o o==o==o o==o |
| // o--o--o o--o--o o--o o--o--o |
| // |
| // Parameter block additions are tested as listed above; expected successful |
| // ones marked with o==o and aliasing ones marked with o--o. |
| |
| Problem problem; |
| problem.AddParameterBlock(IntToPtr(5), 5); // x |
| problem.AddParameterBlock(IntToPtr(13), 3); // y |
| |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 2), |
| "Aliasing detected"); |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 3), |
| "Aliasing detected"); |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 9), |
| "Aliasing detected"); |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 8), 3), |
| "Aliasing detected"); |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(12), 2), |
| "Aliasing detected"); |
| EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(14), 3), |
| "Aliasing detected"); |
| |
| // These ones should work. |
| problem.AddParameterBlock(IntToPtr( 2), 3); |
| problem.AddParameterBlock(IntToPtr(10), 3); |
| problem.AddParameterBlock(IntToPtr(16), 2); |
| |
| ASSERT_EQ(5, problem.NumParameterBlocks()); |
| } |
| |
| TEST(Problem, AddParameterIgnoresDuplicateCalls) { |
| double x[3], y[4]; |
| |
| Problem problem; |
| problem.AddParameterBlock(x, 3); |
| problem.AddParameterBlock(y, 4); |
| |
| // Creating parameter blocks multiple times is ignored. |
| problem.AddParameterBlock(x, 3); |
| problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); |
| |
| // ... even repeatedly. |
| problem.AddParameterBlock(x, 3); |
| problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); |
| |
| // More parameters are fine. |
| problem.AddParameterBlock(y, 4); |
| problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y); |
| |
| EXPECT_EQ(2, problem.NumParameterBlocks()); |
| EXPECT_EQ(7, problem.NumParameters()); |
| } |
| |
| TEST(Problem, AddingParametersAndResidualsResultsInExpectedProblem) { |
| double x[3], y[4], z[5], w[4]; |
| |
| Problem problem; |
| problem.AddParameterBlock(x, 3); |
| EXPECT_EQ(1, problem.NumParameterBlocks()); |
| EXPECT_EQ(3, problem.NumParameters()); |
| |
| problem.AddParameterBlock(y, 4); |
| EXPECT_EQ(2, problem.NumParameterBlocks()); |
| EXPECT_EQ(7, problem.NumParameters()); |
| |
| problem.AddParameterBlock(z, 5); |
| EXPECT_EQ(3, problem.NumParameterBlocks()); |
| EXPECT_EQ(12, problem.NumParameters()); |
| |
| // Add a parameter that has a local parameterization. |
| w[0] = 1.0; w[1] = 0.0; w[2] = 0.0; w[3] = 0.0; |
| problem.AddParameterBlock(w, 4, new QuaternionParameterization); |
| EXPECT_EQ(4, problem.NumParameterBlocks()); |
| EXPECT_EQ(16, problem.NumParameters()); |
| |
| problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); |
| problem.AddResidualBlock(new BinaryCostFunction(6, 5, 4) , NULL, z, y); |
| problem.AddResidualBlock(new BinaryCostFunction(3, 3, 5), NULL, x, z); |
| problem.AddResidualBlock(new BinaryCostFunction(7, 5, 3), NULL, z, x); |
| problem.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), NULL, z, x, y); |
| |
| const int total_residuals = 2 + 6 + 3 + 7 + 1; |
| EXPECT_EQ(problem.NumResidualBlocks(), 5); |
| EXPECT_EQ(problem.NumResiduals(), total_residuals); |
| } |
| |
| class DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> { |
| public: |
| explicit DestructorCountingCostFunction(int *num_destructions) |
| : num_destructions_(num_destructions) {} |
| |
| virtual ~DestructorCountingCostFunction() { |
| *num_destructions_ += 1; |
| } |
| |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| return true; |
| } |
| |
| private: |
| int* num_destructions_; |
| }; |
| |
| TEST(Problem, ReusedCostFunctionsAreOnlyDeletedOnce) { |
| double y[4], z[5]; |
| int num_destructions = 0; |
| |
| // Add a cost function multiple times and check to make sure that |
| // the destructor on the cost function is only called once. |
| { |
| Problem problem; |
| problem.AddParameterBlock(y, 4); |
| problem.AddParameterBlock(z, 5); |
| |
| CostFunction* cost = new DestructorCountingCostFunction(&num_destructions); |
| problem.AddResidualBlock(cost, NULL, y, z); |
| problem.AddResidualBlock(cost, NULL, y, z); |
| problem.AddResidualBlock(cost, NULL, y, z); |
| EXPECT_EQ(3, problem.NumResidualBlocks()); |
| } |
| |
| // Check that the destructor was called only once. |
| CHECK_EQ(num_destructions, 1); |
| } |
| |
| TEST(Problem, CostFunctionsAreDeletedEvenWithRemovals) { |
| double y[4], z[5], w[4]; |
| int num_destructions = 0; |
| { |
| Problem problem; |
| problem.AddParameterBlock(y, 4); |
| problem.AddParameterBlock(z, 5); |
| |
| CostFunction* cost_yz = |
| new DestructorCountingCostFunction(&num_destructions); |
| CostFunction* cost_wz = |
| new DestructorCountingCostFunction(&num_destructions); |
| ResidualBlock* r_yz = problem.AddResidualBlock(cost_yz, NULL, y, z); |
| ResidualBlock* r_wz = problem.AddResidualBlock(cost_wz, NULL, w, z); |
| EXPECT_EQ(2, problem.NumResidualBlocks()); |
| |
| // In the current implementation, the destructor shouldn't get run yet. |
| problem.RemoveResidualBlock(r_yz); |
| CHECK_EQ(num_destructions, 0); |
| problem.RemoveResidualBlock(r_wz); |
| CHECK_EQ(num_destructions, 0); |
| |
| EXPECT_EQ(0, problem.NumResidualBlocks()); |
| } |
| CHECK_EQ(num_destructions, 2); |
| } |
| |
| // Make the dynamic problem tests (e.g. for removing residual blocks) |
| // parameterized on whether the low-latency mode is enabled or not. |
| // |
| // This tests against ProblemImpl instead of Problem in order to inspect the |
| // state of the resulting Program; this is difficult with only the thin Problem |
| // interface. |
| struct DynamicProblem : public ::testing::TestWithParam<bool> { |
| DynamicProblem() { |
| Problem::Options options; |
| options.enable_fast_parameter_block_removal = GetParam(); |
| problem.reset(new ProblemImpl(options)); |
| } |
| |
| ParameterBlock* GetParameterBlock(int block) { |
| return problem->program().parameter_blocks()[block]; |
| } |
| ResidualBlock* GetResidualBlock(int block) { |
| return problem->program().residual_blocks()[block]; |
| } |
| |
| bool HasResidualBlock(ResidualBlock* residual_block) { |
| return find(problem->program().residual_blocks().begin(), |
| problem->program().residual_blocks().end(), |
| residual_block) != problem->program().residual_blocks().end(); |
| } |
| |
| // The next block of functions until the end are only for testing the |
| // residual block removals. |
| void ExpectParameterBlockContainsResidualBlock( |
| double* values, |
| ResidualBlock* residual_block) { |
| ParameterBlock* parameter_block = |
| FindOrDie(problem->parameter_map(), values); |
| EXPECT_TRUE(ContainsKey(*(parameter_block->mutable_residual_blocks()), |
| residual_block)); |
| } |
| |
| void ExpectSize(double* values, int size) { |
| ParameterBlock* parameter_block = |
| FindOrDie(problem->parameter_map(), values); |
| EXPECT_EQ(size, parameter_block->mutable_residual_blocks()->size()); |
| } |
| |
| // Degenerate case. |
| void ExpectParameterBlockContains(double* values) { |
| ExpectSize(values, 0); |
| } |
| |
| void ExpectParameterBlockContains(double* values, |
| ResidualBlock* r1) { |
| ExpectSize(values, 1); |
| ExpectParameterBlockContainsResidualBlock(values, r1); |
| } |
| |
| void ExpectParameterBlockContains(double* values, |
| ResidualBlock* r1, |
| ResidualBlock* r2) { |
| ExpectSize(values, 2); |
| ExpectParameterBlockContainsResidualBlock(values, r1); |
| ExpectParameterBlockContainsResidualBlock(values, r2); |
| } |
| |
| void ExpectParameterBlockContains(double* values, |
| ResidualBlock* r1, |
| ResidualBlock* r2, |
| ResidualBlock* r3) { |
| ExpectSize(values, 3); |
| ExpectParameterBlockContainsResidualBlock(values, r1); |
| ExpectParameterBlockContainsResidualBlock(values, r2); |
| ExpectParameterBlockContainsResidualBlock(values, r3); |
| } |
| |
| void ExpectParameterBlockContains(double* values, |
| ResidualBlock* r1, |
| ResidualBlock* r2, |
| ResidualBlock* r3, |
| ResidualBlock* r4) { |
| ExpectSize(values, 4); |
| ExpectParameterBlockContainsResidualBlock(values, r1); |
| ExpectParameterBlockContainsResidualBlock(values, r2); |
| ExpectParameterBlockContainsResidualBlock(values, r3); |
| ExpectParameterBlockContainsResidualBlock(values, r4); |
| } |
| |
| scoped_ptr<ProblemImpl> problem; |
| double y[4], z[5], w[3]; |
| }; |
| |
| TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { |
| problem->AddParameterBlock(y, 4); |
| problem->AddParameterBlock(z, 5); |
| problem->AddParameterBlock(w, 3); |
| ASSERT_EQ(3, problem->NumParameterBlocks()); |
| ASSERT_EQ(0, problem->NumResidualBlocks()); |
| EXPECT_EQ(y, GetParameterBlock(0)->user_state()); |
| EXPECT_EQ(z, GetParameterBlock(1)->user_state()); |
| EXPECT_EQ(w, GetParameterBlock(2)->user_state()); |
| |
| // w is at the end, which might break the swapping logic so try adding and |
| // removing it. |
| problem->RemoveParameterBlock(w); |
| ASSERT_EQ(2, problem->NumParameterBlocks()); |
| ASSERT_EQ(0, problem->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()); |
| EXPECT_EQ(y, GetParameterBlock(0)->user_state()); |
| EXPECT_EQ(z, GetParameterBlock(1)->user_state()); |
| EXPECT_EQ(w, GetParameterBlock(2)->user_state()); |
| |
| // 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()); |
| 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()); |
| EXPECT_EQ(y, GetParameterBlock(0)->user_state()); |
| EXPECT_EQ(w, GetParameterBlock(1)->user_state()); |
| EXPECT_EQ(z, GetParameterBlock(2)->user_state()); |
| |
| // Now remove everything. |
| // y |
| problem->RemoveParameterBlock(y); |
| ASSERT_EQ(2, problem->NumParameterBlocks()); |
| ASSERT_EQ(0, problem->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()); |
| EXPECT_EQ(w, GetParameterBlock(0)->user_state()); |
| |
| // w |
| problem->RemoveParameterBlock(w); |
| EXPECT_EQ(0, problem->NumParameterBlocks()); |
| EXPECT_EQ(0, problem->NumResidualBlocks()); |
| } |
| |
| TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { |
| problem->AddParameterBlock(y, 4); |
| problem->AddParameterBlock(z, 5); |
| problem->AddParameterBlock(w, 3); |
| ASSERT_EQ(3, problem->NumParameterBlocks()); |
| ASSERT_EQ(0, problem->NumResidualBlocks()); |
| EXPECT_EQ(y, GetParameterBlock(0)->user_state()); |
| EXPECT_EQ(z, GetParameterBlock(1)->user_state()); |
| EXPECT_EQ(w, GetParameterBlock(2)->user_state()); |
| |
| // 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); |
| |
| EXPECT_EQ(3, problem->NumParameterBlocks()); |
| EXPECT_EQ(7, problem->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_FALSE(HasResidualBlock(r_yzw)); |
| ASSERT_TRUE (HasResidualBlock(r_yz )); |
| ASSERT_FALSE(HasResidualBlock(r_yw )); |
| ASSERT_FALSE(HasResidualBlock(r_zw )); |
| ASSERT_TRUE (HasResidualBlock(r_y )); |
| ASSERT_TRUE (HasResidualBlock(r_z )); |
| ASSERT_FALSE(HasResidualBlock(r_w )); |
| |
| // Remove z, which will remove almost everything else. |
| problem->RemoveParameterBlock(z); |
| ASSERT_EQ(1, problem->NumParameterBlocks()); |
| ASSERT_EQ(1, problem->NumResidualBlocks()); |
| |
| ASSERT_FALSE(HasResidualBlock(r_yzw)); |
| ASSERT_FALSE(HasResidualBlock(r_yz )); |
| ASSERT_FALSE(HasResidualBlock(r_yw )); |
| ASSERT_FALSE(HasResidualBlock(r_zw )); |
| ASSERT_TRUE (HasResidualBlock(r_y )); |
| ASSERT_FALSE(HasResidualBlock(r_z )); |
| ASSERT_FALSE(HasResidualBlock(r_w )); |
| |
| // Remove y; all gone. |
| problem->RemoveParameterBlock(y); |
| EXPECT_EQ(0, problem->NumParameterBlocks()); |
| EXPECT_EQ(0, problem->NumResidualBlocks()); |
| } |
| |
| TEST_P(DynamicProblem, RemoveResidualBlock) { |
| 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); |
| |
| if (GetParam()) { |
| // In this test parameterization, there should be back-pointers from the |
| // parameter blocks to the residual blocks. |
| ExpectParameterBlockContains(y, r_yzw, r_yz, r_yw, r_y); |
| ExpectParameterBlockContains(z, r_yzw, r_yz, r_zw, r_z); |
| ExpectParameterBlockContains(w, r_yzw, r_yw, r_zw, r_w); |
| } else { |
| // Otherwise, nothing. |
| EXPECT_TRUE(GetParameterBlock(0)->mutable_residual_blocks() == NULL); |
| EXPECT_TRUE(GetParameterBlock(1)->mutable_residual_blocks() == NULL); |
| EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL); |
| } |
| EXPECT_EQ(3, problem->NumParameterBlocks()); |
| EXPECT_EQ(7, problem->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()); |
| if (GetParam()) { |
| ExpectParameterBlockContains(y, r_yz, r_yw, r_y); |
| ExpectParameterBlockContains(z, r_yz, r_zw, r_z); |
| ExpectParameterBlockContains(w, r_yw, r_zw, r_w); |
| } |
| ASSERT_TRUE (HasResidualBlock(r_yz )); |
| ASSERT_TRUE (HasResidualBlock(r_yw )); |
| ASSERT_TRUE (HasResidualBlock(r_zw )); |
| ASSERT_TRUE (HasResidualBlock(r_y )); |
| ASSERT_TRUE (HasResidualBlock(r_z )); |
| ASSERT_TRUE (HasResidualBlock(r_w )); |
| |
| // Remove r_yw. |
| problem->RemoveResidualBlock(r_yw); |
| ASSERT_EQ(3, problem->NumParameterBlocks()); |
| ASSERT_EQ(5, problem->NumResidualBlocks()); |
| if (GetParam()) { |
| ExpectParameterBlockContains(y, r_yz, r_y); |
| ExpectParameterBlockContains(z, r_yz, r_zw, r_z); |
| ExpectParameterBlockContains(w, r_zw, r_w); |
| } |
| ASSERT_TRUE (HasResidualBlock(r_yz )); |
| ASSERT_TRUE (HasResidualBlock(r_zw )); |
| ASSERT_TRUE (HasResidualBlock(r_y )); |
| ASSERT_TRUE (HasResidualBlock(r_z )); |
| ASSERT_TRUE (HasResidualBlock(r_w )); |
| |
| // Remove r_zw. |
| problem->RemoveResidualBlock(r_zw); |
| ASSERT_EQ(3, problem->NumParameterBlocks()); |
| ASSERT_EQ(4, problem->NumResidualBlocks()); |
| if (GetParam()) { |
| ExpectParameterBlockContains(y, r_yz, r_y); |
| ExpectParameterBlockContains(z, r_yz, r_z); |
| ExpectParameterBlockContains(w, r_w); |
| } |
| ASSERT_TRUE (HasResidualBlock(r_yz )); |
| ASSERT_TRUE (HasResidualBlock(r_y )); |
| ASSERT_TRUE (HasResidualBlock(r_z )); |
| ASSERT_TRUE (HasResidualBlock(r_w )); |
| |
| // Remove r_w. |
| problem->RemoveResidualBlock(r_w); |
| ASSERT_EQ(3, problem->NumParameterBlocks()); |
| ASSERT_EQ(3, problem->NumResidualBlocks()); |
| if (GetParam()) { |
| ExpectParameterBlockContains(y, r_yz, r_y); |
| ExpectParameterBlockContains(z, r_yz, r_z); |
| ExpectParameterBlockContains(w); |
| } |
| ASSERT_TRUE (HasResidualBlock(r_yz )); |
| ASSERT_TRUE (HasResidualBlock(r_y )); |
| ASSERT_TRUE (HasResidualBlock(r_z )); |
| |
| // Remove r_yz. |
| problem->RemoveResidualBlock(r_yz); |
| ASSERT_EQ(3, problem->NumParameterBlocks()); |
| ASSERT_EQ(2, problem->NumResidualBlocks()); |
| if (GetParam()) { |
| ExpectParameterBlockContains(y, r_y); |
| ExpectParameterBlockContains(z, r_z); |
| ExpectParameterBlockContains(w); |
| } |
| ASSERT_TRUE (HasResidualBlock(r_y )); |
| ASSERT_TRUE (HasResidualBlock(r_z )); |
| |
| // Remove the last two. |
| problem->RemoveResidualBlock(r_z); |
| problem->RemoveResidualBlock(r_y); |
| ASSERT_EQ(3, problem->NumParameterBlocks()); |
| ASSERT_EQ(0, problem->NumResidualBlocks()); |
| if (GetParam()) { |
| ExpectParameterBlockContains(y); |
| ExpectParameterBlockContains(z); |
| ExpectParameterBlockContains(w); |
| } |
| } |
| |
| INSTANTIATE_TEST_CASE_P(OptionsInstantiation, |
| DynamicProblem, |
| ::testing::Values(true, false)); |
| |
| } // namespace internal |
| } // namespace ceres |
