Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
new file mode 100644
index 0000000..3f9f804
--- /dev/null
+++ b/internal/ceres/problem_test.cc
@@ -0,0 +1,1586 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// 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 <memory>
+#include "ceres/casts.h"
+#include "ceres/cost_function.h"
+#include "ceres/crs_matrix.h"
+#include "ceres/evaluator_test_utils.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/loss_function.h"
+#include "ceres/map_util.h"
+#include "ceres/parameter_block.h"
+#include "ceres/program.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/sparse_matrix.h"
+#include "ceres/types.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+using std::vector;
+
+// 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, int32_t 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,
+ int32_t parameter_block1_size,
+ int32_t 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,
+ int32_t parameter_block1_size,
+ int32_t parameter_block2_size,
+ int32_t 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 != nullptr");
+}
+
+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),
+ "num_parameter_blocks");
+}
+
+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, GetCostFunctionForResidualBlock) {
+ double x[3];
+ Problem problem;
+ CostFunction* cost_function = new UnaryCostFunction(2, 3);
+ const ResidualBlockId residual_block =
+ problem.AddResidualBlock(cost_function, NULL, x);
+ EXPECT_EQ(problem.GetCostFunctionForResidualBlock(residual_block),
+ cost_function);
+ EXPECT_TRUE(problem.GetLossFunctionForResidualBlock(residual_block) == NULL);
+}
+
+TEST(Problem, GetLossFunctionForResidualBlock) {
+ double x[3];
+ Problem problem;
+ CostFunction* cost_function = new UnaryCostFunction(2, 3);
+ LossFunction* loss_function = new TrivialLoss();
+ const ResidualBlockId residual_block =
+ problem.AddResidualBlock(cost_function, loss_function, x);
+ EXPECT_EQ(problem.GetCostFunctionForResidualBlock(residual_block),
+ cost_function);
+ EXPECT_EQ(problem.GetLossFunctionForResidualBlock(residual_block),
+ loss_function);
+}
+
+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());
+
+ problem.RemoveResidualBlock(r_yz);
+ CHECK_EQ(num_destructions, 1);
+ problem.RemoveResidualBlock(r_wz);
+ CHECK_EQ(num_destructions, 2);
+
+ 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_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) {
+ 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
+ // 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);
+ }
+
+ std::unique_ptr<ProblemImpl> problem;
+ double y[4], z[5], w[3];
+};
+
+TEST(Problem, SetParameterBlockConstantWithUnknownPtrDies) {
+ double x[3];
+ double y[2];
+
+ Problem problem;
+ problem.AddParameterBlock(x, 3);
+
+ EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockConstant(y),
+ "Parameter block not found:");
+}
+
+TEST(Problem, SetParameterBlockVariableWithUnknownPtrDies) {
+ double x[3];
+ double y[2];
+
+ Problem problem;
+ problem.AddParameterBlock(x, 3);
+
+ EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockVariable(y),
+ "Parameter block not found:");
+}
+
+TEST(Problem, IsParameterBlockConstant) {
+ double x1[3];
+ double x2[3];
+
+ Problem problem;
+ problem.AddParameterBlock(x1, 3);
+ problem.AddParameterBlock(x2, 3);
+
+ EXPECT_FALSE(problem.IsParameterBlockConstant(x1));
+ EXPECT_FALSE(problem.IsParameterBlockConstant(x2));
+
+ problem.SetParameterBlockConstant(x1);
+ EXPECT_TRUE(problem.IsParameterBlockConstant(x1));
+ EXPECT_FALSE(problem.IsParameterBlockConstant(x2));
+
+ problem.SetParameterBlockConstant(x2);
+ EXPECT_TRUE(problem.IsParameterBlockConstant(x1));
+ EXPECT_TRUE(problem.IsParameterBlockConstant(x2));
+
+ problem.SetParameterBlockVariable(x1);
+ EXPECT_FALSE(problem.IsParameterBlockConstant(x1));
+ EXPECT_TRUE(problem.IsParameterBlockConstant(x2));
+}
+
+TEST(Problem, IsParameterBlockConstantWithUnknownPtrDies) {
+ double x[3];
+ double y[2];
+
+ Problem problem;
+ problem.AddParameterBlock(x, 3);
+
+ EXPECT_DEATH_IF_SUPPORTED(problem.IsParameterBlockConstant(y),
+ "Parameter block not found:");
+}
+
+TEST(Problem, SetLocalParameterizationWithUnknownPtrDies) {
+ double x[3];
+ double y[2];
+
+ Problem problem;
+ problem.AddParameterBlock(x, 3);
+
+ EXPECT_DEATH_IF_SUPPORTED(
+ problem.SetParameterization(y, new IdentityParameterization(3)),
+ "Parameter block not found:");
+}
+
+TEST(Problem, RemoveParameterBlockWithUnknownPtrDies) {
+ double x[3];
+ double y[2];
+
+ Problem problem;
+ problem.AddParameterBlock(x, 3);
+
+ EXPECT_DEATH_IF_SUPPORTED(
+ 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];
+ Problem problem;
+ problem.AddParameterBlock(x, 3);
+ problem.AddParameterBlock(y, 4);
+
+ vector<int> constant_parameters;
+ constant_parameters.push_back(0);
+ problem.SetParameterization(
+ x,
+ new SubsetParameterization(3, constant_parameters));
+ EXPECT_EQ(problem.ParameterBlockSize(x), 3);
+ EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2);
+ EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4);
+
+ vector<double*> parameter_blocks;
+ problem.GetParameterBlocks(¶meter_blocks);
+ EXPECT_EQ(parameter_blocks.size(), 2);
+ EXPECT_NE(parameter_blocks[0], parameter_blocks[1]);
+ 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);
+}
+
+TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) {
+ problem->AddParameterBlock(y, 4);
+ problem->AddParameterBlock(z, 5);
+ problem->AddParameterBlock(w, 3);
+ ASSERT_EQ(3, problem->NumParameterBlocks());
+ 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());
+
+ // 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, 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, 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, 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, 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, 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, NumResidualBlocks());
+ EXPECT_EQ(w, GetParameterBlock(0)->user_state());
+
+ // w
+ problem->RemoveParameterBlock(w);
+ EXPECT_EQ(0, problem->NumParameterBlocks());
+ EXPECT_EQ(0, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, NumResidualBlocks());
+ if (GetParam()) {
+ ExpectParameterBlockContains(y);
+ ExpectParameterBlockContains(z);
+ ExpectParameterBlockContains(w);
+ }
+}
+
+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));
+
+// Test for Problem::Evaluate
+
+// r_i = i - (j + 1) * x_ij^2
+template <int kNumResiduals, int kNumParameterBlocks>
+class QuadraticCostFunction : public CostFunction {
+ public:
+ QuadraticCostFunction() {
+ CHECK_GT(kNumResiduals, 0);
+ CHECK_GT(kNumParameterBlocks, 0);
+ set_num_residuals(kNumResiduals);
+ for (int i = 0; i < kNumParameterBlocks; ++i) {
+ mutable_parameter_block_sizes()->push_back(kNumResiduals);
+ }
+ }
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ for (int i = 0; i < kNumResiduals; ++i) {
+ residuals[i] = i;
+ for (int j = 0; j < kNumParameterBlocks; ++j) {
+ residuals[i] -= (j + 1.0) * parameters[j][i] * parameters[j][i];
+ }
+ }
+
+ if (jacobians == NULL) {
+ return true;
+ }
+
+ for (int j = 0; j < kNumParameterBlocks; ++j) {
+ if (jacobians[j] != NULL) {
+ MatrixRef(jacobians[j], kNumResiduals, kNumResiduals) =
+ (-2.0 * (j + 1.0) *
+ ConstVectorRef(parameters[j], kNumResiduals)).asDiagonal();
+ }
+ }
+
+ return true;
+ }
+};
+
+// Convert a CRSMatrix to a dense Eigen matrix.
+void CRSToDenseMatrix(const CRSMatrix& input, Matrix* output) {
+ CHECK(output != nullptr);
+ Matrix& m = *output;
+ m.resize(input.num_rows, input.num_cols);
+ m.setZero();
+ for (int row = 0; row < input.num_rows; ++row) {
+ for (int j = input.rows[row]; j < input.rows[row + 1]; ++j) {
+ const int col = input.cols[j];
+ m(row, col) = input.values[j];
+ }
+ }
+}
+
+class ProblemEvaluateTest : public ::testing::Test {
+ protected:
+ void SetUp() {
+ for (int i = 0; i < 6; ++i) {
+ parameters_[i] = static_cast<double>(i + 1);
+ }
+
+ parameter_blocks_.push_back(parameters_);
+ parameter_blocks_.push_back(parameters_ + 2);
+ parameter_blocks_.push_back(parameters_ + 4);
+
+
+ CostFunction* cost_function = new QuadraticCostFunction<2, 2>;
+
+ // f(x, y)
+ residual_blocks_.push_back(
+ problem_.AddResidualBlock(cost_function,
+ NULL,
+ parameters_,
+ parameters_ + 2));
+ // g(y, z)
+ residual_blocks_.push_back(
+ problem_.AddResidualBlock(cost_function,
+ NULL, parameters_ + 2,
+ parameters_ + 4));
+ // h(z, x)
+ residual_blocks_.push_back(
+ problem_.AddResidualBlock(cost_function,
+ NULL,
+ parameters_ + 4,
+ parameters_));
+ }
+
+ void TearDown() {
+ EXPECT_TRUE(problem_.program().IsValid());
+ }
+
+ void EvaluateAndCompare(const Problem::EvaluateOptions& options,
+ const int expected_num_rows,
+ const int expected_num_cols,
+ const double expected_cost,
+ const double* expected_residuals,
+ const double* expected_gradient,
+ const double* expected_jacobian) {
+ double cost;
+ vector<double> residuals;
+ vector<double> gradient;
+ CRSMatrix jacobian;
+
+ EXPECT_TRUE(
+ problem_.Evaluate(options,
+ &cost,
+ expected_residuals != NULL ? &residuals : NULL,
+ expected_gradient != NULL ? &gradient : NULL,
+ expected_jacobian != NULL ? &jacobian : NULL));
+
+ if (expected_residuals != NULL) {
+ EXPECT_EQ(residuals.size(), expected_num_rows);
+ }
+
+ if (expected_gradient != NULL) {
+ EXPECT_EQ(gradient.size(), expected_num_cols);
+ }
+
+ if (expected_jacobian != NULL) {
+ EXPECT_EQ(jacobian.num_rows, expected_num_rows);
+ EXPECT_EQ(jacobian.num_cols, expected_num_cols);
+ }
+
+ Matrix dense_jacobian;
+ if (expected_jacobian != NULL) {
+ CRSToDenseMatrix(jacobian, &dense_jacobian);
+ }
+
+ CompareEvaluations(expected_num_rows,
+ expected_num_cols,
+ expected_cost,
+ expected_residuals,
+ expected_gradient,
+ expected_jacobian,
+ cost,
+ residuals.size() > 0 ? &residuals[0] : NULL,
+ gradient.size() > 0 ? &gradient[0] : NULL,
+ dense_jacobian.data());
+ }
+
+ void CheckAllEvaluationCombinations(const Problem::EvaluateOptions& options,
+ const ExpectedEvaluation& expected) {
+ for (int i = 0; i < 8; ++i) {
+ EvaluateAndCompare(options,
+ expected.num_rows,
+ expected.num_cols,
+ expected.cost,
+ (i & 1) ? expected.residuals : NULL,
+ (i & 2) ? expected.gradient : NULL,
+ (i & 4) ? expected.jacobian : NULL);
+ }
+ }
+
+ ProblemImpl problem_;
+ double parameters_[6];
+ vector<double*> parameter_blocks_;
+ vector<ResidualBlockId> residual_blocks_;
+};
+
+
+TEST_F(ProblemEvaluateTest, MultipleParameterAndResidualBlocks) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 6,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -59.0, -87.0, // g
+ -27.0, -43.0 // h
+ },
+ // Gradient
+ { 146.0, 484.0, // x
+ 582.0, 1256.0, // y
+ 1450.0, 2604.0, // z
+ },
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, -8.0, 0.0, -24.0,
+ /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
+ }
+ };
+
+ CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
+}
+
+TEST_F(ProblemEvaluateTest, ParameterAndResidualBlocksPassedInOptions) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 6,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -59.0, -87.0, // g
+ -27.0, -43.0 // h
+ },
+ // Gradient
+ { 146.0, 484.0, // x
+ 582.0, 1256.0, // y
+ 1450.0, 2604.0, // z
+ },
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, -8.0, 0.0, -24.0,
+ /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ evaluate_options.parameter_blocks = parameter_blocks_;
+ evaluate_options.residual_blocks = residual_blocks_;
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ReorderedResidualBlocks) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 6,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -27.0, -43.0, // h
+ -59.0, -87.0 // g
+ },
+ // Gradient
+ { 146.0, 484.0, // x
+ 582.0, 1256.0, // y
+ 1450.0, 2604.0, // z
+ },
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
+ /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, 0.0, 0.0, -12.0,
+ /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, -8.0, 0.0, -24.0
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ evaluate_options.parameter_blocks = parameter_blocks_;
+
+ // f, h, g
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ReorderedResidualBlocksAndReorderedParameterBlocks) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 6,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -27.0, -43.0, // h
+ -59.0, -87.0 // g
+ },
+ // Gradient
+ { 1450.0, 2604.0, // z
+ 582.0, 1256.0, // y
+ 146.0, 484.0, // x
+ },
+ // Jacobian
+ // z y x
+ { /* f(x, y) */ 0.0, 0.0, -12.0, 0.0, -2.0, 0.0,
+ 0.0, 0.0, 0.0, -16.0, 0.0, -4.0,
+ /* h(z, x) */ -10.0, 0.0, 0.0, 0.0, -4.0, 0.0,
+ 0.0, -12.0, 0.0, 0.0, 0.0, -8.0,
+ /* g(y, z) */ -20.0, 0.0, -6.0, 0.0, 0.0, 0.0,
+ 0.0, -24.0, 0.0, -8.0, 0.0, 0.0
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ // z, y, x
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[1]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
+
+ // f, h, g
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ConstantParameterBlock) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 6,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -59.0, -87.0, // g
+ -27.0, -43.0 // h
+ },
+
+ // Gradient
+ { 146.0, 484.0, // x
+ 0.0, 0.0, // y
+ 1450.0, 2604.0, // z
+ },
+
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, 0.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, 0.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.0, -24.0,
+ /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
+ }
+ };
+
+ problem_.SetParameterBlockConstant(parameters_ + 2);
+ CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
+}
+
+TEST_F(ProblemEvaluateTest, ExcludedAResidualBlock) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 4, 6,
+ // Cost
+ 2082.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -27.0, -43.0 // h
+ },
+ // Gradient
+ { 146.0, 484.0, // x
+ 228.0, 560.0, // y
+ 270.0, 516.0, // z
+ },
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
+ /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ExcludedParameterBlock) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 4,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -59.0, -87.0, // g
+ -27.0, -43.0 // h
+ },
+
+ // Gradient
+ { 146.0, 484.0, // x
+ 1450.0, 2604.0, // z
+ },
+
+ // Jacobian
+ // x z
+ { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, -24.0,
+ /* h(z, x) */ -4.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, -12.0
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ // x, z
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
+ evaluate_options.residual_blocks = residual_blocks_;
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ExcludedParameterBlockAndExcludedResidualBlock) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 4, 4,
+ // Cost
+ 6318.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -59.0, -87.0, // g
+ },
+
+ // Gradient
+ { 38.0, 140.0, // x
+ 1180.0, 2088.0, // z
+ },
+
+ // Jacobian
+ // x z
+ { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, -24.0,
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ // x, z
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, LocalParameterization) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 5,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -59.0, -87.0, // g
+ -27.0, -43.0 // h
+ },
+ // Gradient
+ { 146.0, 484.0, // x
+ 1256.0, // y with SubsetParameterization
+ 1450.0, 2604.0, // z
+ },
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, -16.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, -8.0, 0.0, -24.0,
+ /* h(z, x) */ -4.0, 0.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, 0.0, -12.0
+ }
+ };
+
+ vector<int> constant_parameters;
+ constant_parameters.push_back(0);
+ problem_.SetParameterization(parameters_ + 2,
+ new SubsetParameterization(2,
+ constant_parameters));
+
+ CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
+}
+
+TEST(Problem, SetAndGetParameterLowerBound) {
+ Problem problem;
+ double x[] = {1.0, 2.0};
+ problem.AddParameterBlock(x, 2);
+
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 0),
+ -std::numeric_limits<double>::max());
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
+ -std::numeric_limits<double>::max());
+
+ problem.SetParameterLowerBound(x, 0, -1.0);
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 0), -1.0);
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
+ -std::numeric_limits<double>::max());
+
+ problem.SetParameterLowerBound(x, 0, -2.0);
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 0), -2.0);
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
+ -std::numeric_limits<double>::max());
+
+ problem.SetParameterLowerBound(x, 0, -std::numeric_limits<double>::max());
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 0),
+ -std::numeric_limits<double>::max());
+ EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
+ -std::numeric_limits<double>::max());
+}
+
+TEST(Problem, SetAndGetParameterUpperBound) {
+ Problem problem;
+ double x[] = {1.0, 2.0};
+ problem.AddParameterBlock(x, 2);
+
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 0),
+ std::numeric_limits<double>::max());
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
+ std::numeric_limits<double>::max());
+
+ problem.SetParameterUpperBound(x, 0, -1.0);
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 0), -1.0);
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
+ std::numeric_limits<double>::max());
+
+ problem.SetParameterUpperBound(x, 0, -2.0);
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 0), -2.0);
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
+ std::numeric_limits<double>::max());
+
+ problem.SetParameterUpperBound(x, 0, std::numeric_limits<double>::max());
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 0),
+ std::numeric_limits<double>::max());
+ EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
+ std::numeric_limits<double>::max());
+}
+
+} // namespace internal
+} // namespace ceres