blob: 300a3a54c5998b328c72e091a7cd861a768282ab [file] [log] [blame]
// 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)
#include "ceres/program.h"
#include <cmath>
#include <limits>
#include <memory>
#include <utility>
#include <vector>
#include "ceres/internal/integer_sequence_algorithm.h"
#include "ceres/problem_impl.h"
#include "ceres/residual_block.h"
#include "ceres/sized_cost_function.h"
#include "ceres/triplet_sparse_matrix.h"
#include "gtest/gtest.h"
namespace ceres {
namespace internal {
using std::string;
using std::vector;
// A cost function that simply returns its argument.
class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
public:
bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const final {
residuals[0] = parameters[0][0];
if (jacobians != nullptr && jacobians[0] != nullptr) {
jacobians[0][0] = 1.0;
}
return true;
}
};
// Templated base class for the CostFunction signatures.
template <int kNumResiduals, int... Ns>
class MockCostFunctionBase : public SizedCostFunction<kNumResiduals, Ns...> {
public:
bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const final {
constexpr int kNumParameters = (Ns + ... + 0);
for (int i = 0; i < kNumResiduals; ++i) {
residuals[i] = kNumResiduals + kNumParameters;
}
return true;
}
};
class UnaryCostFunction : public MockCostFunctionBase<2, 1> {};
class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1> {};
class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
TEST(Program, RemoveFixedBlocksNothingConstant) {
ProblemImpl problem;
double x;
double y;
double z;
problem.AddParameterBlock(&x, 1);
problem.AddParameterBlock(&y, 1);
problem.AddParameterBlock(&z, 1);
problem.AddResidualBlock(new UnaryCostFunction(), nullptr, &x);
problem.AddResidualBlock(new BinaryCostFunction(), nullptr, &x, &y);
problem.AddResidualBlock(new TernaryCostFunction(), nullptr, &x, &y, &z);
vector<double*> removed_parameter_blocks;
double fixed_cost = 0.0;
string message;
std::unique_ptr<Program> reduced_program(
problem.program().CreateReducedProgram(
&removed_parameter_blocks, &fixed_cost, &message));
EXPECT_EQ(reduced_program->NumParameterBlocks(), 3);
EXPECT_EQ(reduced_program->NumResidualBlocks(), 3);
EXPECT_EQ(removed_parameter_blocks.size(), 0);
EXPECT_EQ(fixed_cost, 0.0);
}
TEST(Program, RemoveFixedBlocksAllParameterBlocksConstant) {
ProblemImpl problem;
double x = 1.0;
problem.AddParameterBlock(&x, 1);
problem.AddResidualBlock(new UnaryCostFunction(), nullptr, &x);
problem.SetParameterBlockConstant(&x);
vector<double*> removed_parameter_blocks;
double fixed_cost = 0.0;
string message;
std::unique_ptr<Program> reduced_program(
problem.program().CreateReducedProgram(
&removed_parameter_blocks, &fixed_cost, &message));
EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
EXPECT_EQ(removed_parameter_blocks.size(), 1);
EXPECT_EQ(removed_parameter_blocks[0], &x);
EXPECT_EQ(fixed_cost, 9.0);
}
TEST(Program, RemoveFixedBlocksNoResidualBlocks) {
ProblemImpl problem;
double x;
double y;
double z;
problem.AddParameterBlock(&x, 1);
problem.AddParameterBlock(&y, 1);
problem.AddParameterBlock(&z, 1);
vector<double*> removed_parameter_blocks;
double fixed_cost = 0.0;
string message;
std::unique_ptr<Program> reduced_program(
problem.program().CreateReducedProgram(
&removed_parameter_blocks, &fixed_cost, &message));
EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
EXPECT_EQ(removed_parameter_blocks.size(), 3);
EXPECT_EQ(fixed_cost, 0.0);
}
TEST(Program, RemoveFixedBlocksOneParameterBlockConstant) {
ProblemImpl problem;
double x;
double y;
double z;
problem.AddParameterBlock(&x, 1);
problem.AddParameterBlock(&y, 1);
problem.AddParameterBlock(&z, 1);
problem.AddResidualBlock(new UnaryCostFunction(), nullptr, &x);
problem.AddResidualBlock(new BinaryCostFunction(), nullptr, &x, &y);
problem.SetParameterBlockConstant(&x);
vector<double*> removed_parameter_blocks;
double fixed_cost = 0.0;
string message;
std::unique_ptr<Program> reduced_program(
problem.program().CreateReducedProgram(
&removed_parameter_blocks, &fixed_cost, &message));
EXPECT_EQ(reduced_program->NumParameterBlocks(), 1);
EXPECT_EQ(reduced_program->NumResidualBlocks(), 1);
}
TEST(Program, RemoveFixedBlocksNumEliminateBlocks) {
ProblemImpl problem;
double x;
double y;
double z;
problem.AddParameterBlock(&x, 1);
problem.AddParameterBlock(&y, 1);
problem.AddParameterBlock(&z, 1);
problem.AddResidualBlock(new UnaryCostFunction(), nullptr, &x);
problem.AddResidualBlock(new TernaryCostFunction(), nullptr, &x, &y, &z);
problem.AddResidualBlock(new BinaryCostFunction(), nullptr, &x, &y);
problem.SetParameterBlockConstant(&x);
vector<double*> removed_parameter_blocks;
double fixed_cost = 0.0;
string message;
std::unique_ptr<Program> reduced_program(
problem.program().CreateReducedProgram(
&removed_parameter_blocks, &fixed_cost, &message));
EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
}
TEST(Program, RemoveFixedBlocksFixedCost) {
ProblemImpl problem;
double x = 1.23;
double y = 4.56;
double z = 7.89;
problem.AddParameterBlock(&x, 1);
problem.AddParameterBlock(&y, 1);
problem.AddParameterBlock(&z, 1);
problem.AddResidualBlock(new UnaryIdentityCostFunction(), nullptr, &x);
problem.AddResidualBlock(new TernaryCostFunction(), nullptr, &x, &y, &z);
problem.AddResidualBlock(new BinaryCostFunction(), nullptr, &x, &y);
problem.SetParameterBlockConstant(&x);
ResidualBlock* expected_removed_block =
problem.program().residual_blocks()[0];
std::unique_ptr<double[]> scratch(
new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
double expected_fixed_cost;
expected_removed_block->Evaluate(
true, &expected_fixed_cost, nullptr, nullptr, scratch.get());
vector<double*> removed_parameter_blocks;
double fixed_cost = 0.0;
string message;
std::unique_ptr<Program> reduced_program(
problem.program().CreateReducedProgram(
&removed_parameter_blocks, &fixed_cost, &message));
EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
}
class BlockJacobianTest : public ::testing::TestWithParam<int> {};
TEST_P(BlockJacobianTest, CreateJacobianBlockSparsityTranspose) {
ProblemImpl problem;
double x[2];
double y[3];
double z;
problem.AddParameterBlock(x, 2);
problem.AddParameterBlock(y, 3);
problem.AddParameterBlock(&z, 1);
problem.AddResidualBlock(new MockCostFunctionBase<2, 2>(), nullptr, x);
problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2>(), nullptr, &z, x);
problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3>(), nullptr, &z, y);
problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3>(), nullptr, &z, y);
problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1>(), nullptr, x, &z);
problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3>(), nullptr, &z, y);
problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1>(), nullptr, x, &z);
problem.AddResidualBlock(new MockCostFunctionBase<1, 3>(), nullptr, y);
TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
{
int* rows = expected_block_sparse_jacobian.mutable_rows();
int* cols = expected_block_sparse_jacobian.mutable_cols();
double* values = expected_block_sparse_jacobian.mutable_values();
rows[0] = 0;
cols[0] = 0;
rows[1] = 2;
cols[1] = 1;
rows[2] = 0;
cols[2] = 1;
rows[3] = 2;
cols[3] = 2;
rows[4] = 1;
cols[4] = 2;
rows[5] = 2;
cols[5] = 3;
rows[6] = 1;
cols[6] = 3;
rows[7] = 0;
cols[7] = 4;
rows[8] = 2;
cols[8] = 4;
rows[9] = 2;
cols[9] = 5;
rows[10] = 1;
cols[10] = 5;
rows[11] = 0;
cols[11] = 6;
rows[12] = 2;
cols[12] = 6;
rows[13] = 1;
cols[13] = 7;
std::fill(values, values + 14, 1.0);
expected_block_sparse_jacobian.set_num_nonzeros(14);
}
Program* program = problem.mutable_program();
program->SetParameterOffsetsAndIndex();
const int start_row_block = GetParam();
std::unique_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
program->CreateJacobianBlockSparsityTranspose(start_row_block));
Matrix expected_full_dense_jacobian;
expected_block_sparse_jacobian.ToDenseMatrix(&expected_full_dense_jacobian);
Matrix expected_dense_jacobian =
expected_full_dense_jacobian.rightCols(8 - start_row_block);
Matrix actual_dense_jacobian;
actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
EXPECT_EQ(expected_dense_jacobian.rows(), actual_dense_jacobian.rows());
EXPECT_EQ(expected_dense_jacobian.cols(), actual_dense_jacobian.cols());
EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
}
INSTANTIATE_TEST_SUITE_P(AllColumns, BlockJacobianTest, ::testing::Range(0, 7));
template <int kNumResiduals, int kNumParameterBlocks>
class NumParameterBlocksCostFunction : public CostFunction {
public:
NumParameterBlocksCostFunction() {
set_num_residuals(kNumResiduals);
for (int i = 0; i < kNumParameterBlocks; ++i) {
mutable_parameter_block_sizes()->push_back(1);
}
}
bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const final {
return true;
}
};
TEST(Program, ReallocationInCreateJacobianBlockSparsityTranspose) {
// CreateJacobianBlockSparsityTranspose starts with a conservative
// estimate of the size of the sparsity pattern. This test ensures
// that when those estimates are violated, the reallocation/resizing
// logic works correctly.
ProblemImpl problem;
double x[20];
vector<double*> parameter_blocks;
for (int i = 0; i < 20; ++i) {
problem.AddParameterBlock(x + i, 1);
parameter_blocks.push_back(x + i);
}
problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
nullptr,
parameter_blocks.data(),
static_cast<int>(parameter_blocks.size()));
TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
{
int* rows = expected_block_sparse_jacobian.mutable_rows();
int* cols = expected_block_sparse_jacobian.mutable_cols();
for (int i = 0; i < 20; ++i) {
rows[i] = i;
cols[i] = 0;
}
double* values = expected_block_sparse_jacobian.mutable_values();
std::fill(values, values + 20, 1.0);
expected_block_sparse_jacobian.set_num_nonzeros(20);
}
Program* program = problem.mutable_program();
program->SetParameterOffsetsAndIndex();
std::unique_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
program->CreateJacobianBlockSparsityTranspose());
Matrix expected_dense_jacobian;
expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
Matrix actual_dense_jacobian;
actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
}
TEST(Program, ProblemHasNanParameterBlocks) {
ProblemImpl problem;
double x[2];
x[0] = 1.0;
x[1] = std::numeric_limits<double>::quiet_NaN();
problem.AddResidualBlock(new MockCostFunctionBase<1, 2>(), nullptr, x);
string error;
EXPECT_FALSE(problem.program().ParameterBlocksAreFinite(&error));
EXPECT_NE(error.find("has at least one invalid value"), string::npos)
<< error;
}
TEST(Program, InfeasibleParameterBlock) {
ProblemImpl problem;
double x[] = {0.0, 0.0};
problem.AddResidualBlock(new MockCostFunctionBase<1, 2>(), nullptr, x);
problem.SetParameterLowerBound(x, 0, 2.0);
problem.SetParameterUpperBound(x, 0, 1.0);
string error;
EXPECT_FALSE(problem.program().IsFeasible(&error));
EXPECT_NE(error.find("infeasible bound"), string::npos) << error;
}
TEST(Program, InfeasibleConstantParameterBlock) {
ProblemImpl problem;
double x[] = {0.0, 0.0};
problem.AddResidualBlock(new MockCostFunctionBase<1, 2>(), nullptr, x);
problem.SetParameterLowerBound(x, 0, 1.0);
problem.SetParameterUpperBound(x, 0, 2.0);
problem.SetParameterBlockConstant(x);
string error;
EXPECT_FALSE(problem.program().IsFeasible(&error));
EXPECT_NE(error.find("infeasible value"), string::npos) << error;
}
} // namespace internal
} // namespace ceres