Initial commit of Ceres Solver.
diff --git a/internal/ceres/visibility_based_preconditioner_test.cc b/internal/ceres/visibility_based_preconditioner_test.cc
new file mode 100644
index 0000000..390f081
--- /dev/null
+++ b/internal/ceres/visibility_based_preconditioner_test.cc
@@ -0,0 +1,363 @@
+// 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)
+
+#ifndef CERES_NO_SUITESPARSE
+
+#include "ceres/visibility_based_preconditioner.h"
+
+#include <glog/logging.h>
+#include "ceres/file.h"
+#include "gtest/gtest.h"
+#include "Eigen/Dense"
+#include "ceres/block_random_access_dense_matrix.h"
+#include "ceres/block_random_access_sparse_matrix.h"
+#include "ceres/block_sparse_matrix.h"
+#include "ceres/casts.h"
+#include "ceres/collections_port.h"
+#include "ceres/linear_least_squares_problems.h"
+#include "ceres/schur_eliminator.h"
+#include "ceres/stringprintf.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/types.h"
+
+DECLARE_string(test_srcdir);
+
+namespace ceres {
+namespace internal {
+
+using testing::AssertionResult;
+using testing::AssertionSuccess;
+using testing::AssertionFailure;
+
+static const double kTolerance = 1e-12;
+
+class VisibilityBasedPreconditionerTest : public ::testing::Test {
+ public:
+ static const int kCameraSize = 9;
+
+ protected:
+ void SetUp() {
+ string input_file =
+ JoinPath(FLAGS_test_srcdir,
+ "problem-6-1384-000.lsqp"); // NOLINT
+
+ scoped_ptr<LinearLeastSquaresProblem> problem(
+ CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file)));
+ A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
+ b_.reset(problem->b.release());
+ D_.reset(problem->D.release());
+
+ const CompressedRowBlockStructure* bs =
+ CHECK_NOTNULL(A_->block_structure());
+ const int num_col_blocks = bs->cols.size();
+
+ num_cols_ = A_->num_cols();
+ num_rows_ = A_->num_rows();
+ num_eliminate_blocks_ = problem->num_eliminate_blocks;
+ num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_;
+ options_.num_eliminate_blocks = num_eliminate_blocks_;
+
+ vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
+ for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
+ blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
+ }
+
+ // The input matrix is a real jacobian and fairly poorly
+ // conditioned. Setting D to a large constant makes the normal
+ // equations better conditioned and makes the tests below better
+ // conditioned.
+ VectorRef(D_.get(), num_cols_).setConstant(10.0);
+
+ schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks));
+ Vector rhs(schur_complement_->num_rows());
+
+ scoped_ptr<SchurEliminatorBase> eliminator;
+ eliminator.reset(SchurEliminatorBase::Create(options_));
+ eliminator->Init(num_eliminate_blocks_, bs);
+ eliminator->Eliminate(A_.get(), b_.get(), D_.get(),
+ schur_complement_.get(), rhs.data());
+ }
+
+
+ AssertionResult IsSparsityStructureValid() {
+ preconditioner_->InitStorage(*A_->block_structure());
+ const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
+ const vector<int>& cluster_membership = get_cluster_membership();
+
+ for (int i = 0; i < num_camera_blocks_; ++i) {
+ for (int j = i; j < num_camera_blocks_; ++j) {
+ if (cluster_pairs.count(make_pair(cluster_membership[i],
+ cluster_membership[j]))) {
+ if (!IsBlockPairInPreconditioner(i, j)) {
+ return AssertionFailure()
+ << "block pair (" << i << "," << j << "missing";
+ }
+ } else {
+ if (IsBlockPairInPreconditioner(i, j)) {
+ return AssertionFailure()
+ << "block pair (" << i << "," << j << "should not be present";
+ }
+ }
+ }
+ }
+ return AssertionSuccess();
+ }
+
+ AssertionResult PreconditionerValuesMatch() {
+ preconditioner_->Compute(*A_, D_.get());
+ const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
+ const BlockRandomAccessSparseMatrix* m = get_m();
+ Matrix preconditioner_matrix;
+ m->matrix()->ToDenseMatrix(&preconditioner_matrix);
+ ConstMatrixRef full_schur_complement(schur_complement_->values(),
+ m->num_rows(),
+ m->num_rows());
+ const int num_clusters = get_num_clusters();
+ const int kDiagonalBlockSize =
+ kCameraSize * num_camera_blocks_ / num_clusters;
+
+ for (int i = 0; i < num_clusters; ++i) {
+ for (int j = i; j < num_clusters; ++j) {
+ double diff = 0.0;
+ if (cluster_pairs.count(make_pair(i, j))) {
+ diff =
+ (preconditioner_matrix.block(kDiagonalBlockSize * i,
+ kDiagonalBlockSize * j,
+ kDiagonalBlockSize,
+ kDiagonalBlockSize) -
+ full_schur_complement.block(kDiagonalBlockSize * i,
+ kDiagonalBlockSize * j,
+ kDiagonalBlockSize,
+ kDiagonalBlockSize)).norm();
+ } else {
+ diff = preconditioner_matrix.block(kDiagonalBlockSize * i,
+ kDiagonalBlockSize * j,
+ kDiagonalBlockSize,
+ kDiagonalBlockSize).norm();
+ }
+ if (diff > kTolerance) {
+ return AssertionFailure()
+ << "Preconditioner block " << i << " " << j << " differs "
+ << "from expected value by " << diff;
+ }
+ }
+ }
+ return AssertionSuccess();
+ }
+
+ // Accessors
+ int get_num_blocks() { return preconditioner_->num_blocks_; }
+
+ int get_num_clusters() { return preconditioner_->num_clusters_; }
+ int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; }
+
+ const vector<int>& get_block_size() {
+ return preconditioner_->block_size_; }
+
+ vector<int>* get_mutable_block_size() {
+ return &preconditioner_->block_size_; }
+
+ const vector<int>& get_cluster_membership() {
+ return preconditioner_->cluster_membership_;
+ }
+
+ vector<int>* get_mutable_cluster_membership() {
+ return &preconditioner_->cluster_membership_;
+ }
+
+ const set<pair<int, int> >& get_block_pairs() {
+ return preconditioner_->block_pairs_;
+ }
+
+ set<pair<int, int> >* get_mutable_block_pairs() {
+ return &preconditioner_->block_pairs_;
+ }
+
+ const HashSet<pair<int, int> >& get_cluster_pairs() {
+ return preconditioner_->cluster_pairs_;
+ }
+
+ HashSet<pair<int, int> >* get_mutable_cluster_pairs() {
+ return &preconditioner_->cluster_pairs_;
+ }
+
+ bool IsBlockPairInPreconditioner(const int block1, const int block2) {
+ return preconditioner_->IsBlockPairInPreconditioner(block1, block2);
+ }
+
+ bool IsBlockPairOffDiagonal(const int block1, const int block2) {
+ return preconditioner_->IsBlockPairOffDiagonal(block1, block2);
+ }
+
+ const BlockRandomAccessSparseMatrix* get_m() {
+ return preconditioner_->m_.get();
+ }
+
+ int num_rows_;
+ int num_cols_;
+ int num_eliminate_blocks_;
+ int num_camera_blocks_;
+
+ scoped_ptr<BlockSparseMatrix> A_;
+ scoped_array<double> b_;
+ scoped_array<double> D_;
+
+ LinearSolver::Options options_;
+ scoped_ptr<VisibilityBasedPreconditioner> preconditioner_;
+ scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_;
+};
+
+#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
+TEST_F(VisibilityBasedPreconditionerTest, SchurJacobiStructure) {
+ options_.preconditioner_type = SCHUR_JACOBI;
+ preconditioner_.reset(
+ new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
+ EXPECT_EQ(get_num_blocks(), num_camera_blocks_);
+ EXPECT_EQ(get_num_clusters(), num_camera_blocks_);
+ for (int i = 0; i < num_camera_blocks_; ++i) {
+ for (int j = 0; j < num_camera_blocks_; ++j) {
+ const string msg = StringPrintf("Camera pair: %d %d", i, j);
+ SCOPED_TRACE(msg);
+ if (i == j) {
+ EXPECT_TRUE(IsBlockPairInPreconditioner(i, j));
+ EXPECT_FALSE(IsBlockPairOffDiagonal(i, j));
+ } else {
+ EXPECT_FALSE(IsBlockPairInPreconditioner(i, j));
+ EXPECT_TRUE(IsBlockPairOffDiagonal(i, j));
+ }
+ }
+ }
+}
+
+TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
+ options_.preconditioner_type = CLUSTER_JACOBI;
+ preconditioner_.reset(
+ new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
+
+ // Override the clustering to be a single clustering containing all
+ // the cameras.
+ vector<int>& cluster_membership = *get_mutable_cluster_membership();
+ for (int i = 0; i < num_camera_blocks_; ++i) {
+ cluster_membership[i] = 0;
+ }
+
+ *get_mutable_num_clusters() = 1;
+
+ HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
+ cluster_pairs.clear();
+ cluster_pairs.insert(make_pair(0, 0));
+
+ EXPECT_TRUE(IsSparsityStructureValid());
+ EXPECT_TRUE(PreconditionerValuesMatch());
+
+ // Multiplication by the inverse of the preconditioner.
+ const int num_rows = schur_complement_->num_rows();
+ ConstMatrixRef full_schur_complement(schur_complement_->values(),
+ num_rows,
+ num_rows);
+ Vector x(num_rows);
+ Vector y(num_rows);
+ Vector z(num_rows);
+
+ for (int i = 0; i < num_rows; ++i) {
+ x.setZero();
+ y.setZero();
+ z.setZero();
+ x[i] = 1.0;
+ preconditioner_->RightMultiply(x.data(), y.data());
+ z = full_schur_complement
+ .selfadjointView<Eigen::Upper>()
+ .ldlt().solve(x);
+ double max_relative_difference =
+ ((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>();
+ EXPECT_NEAR(max_relative_difference, 0.0, kTolerance);
+ }
+}
+
+
+
+TEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) {
+ options_.preconditioner_type = CLUSTER_JACOBI;
+ preconditioner_.reset(
+ new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
+
+ // Override the clustering to be equal number of cameras.
+ vector<int>& cluster_membership = *get_mutable_cluster_membership();
+ cluster_membership.resize(num_camera_blocks_);
+ static const int kNumClusters = 3;
+
+ for (int i = 0; i < num_camera_blocks_; ++i) {
+ cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
+ }
+ *get_mutable_num_clusters() = kNumClusters;
+
+ HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
+ cluster_pairs.clear();
+ for (int i = 0; i < kNumClusters; ++i) {
+ cluster_pairs.insert(make_pair(i, i));
+ }
+
+ EXPECT_TRUE(IsSparsityStructureValid());
+ EXPECT_TRUE(PreconditionerValuesMatch());
+}
+
+
+TEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) {
+ options_.preconditioner_type = CLUSTER_TRIDIAGONAL;
+ preconditioner_.reset(
+ new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
+ static const int kNumClusters = 3;
+
+ // Override the clustering to be 3 clusters.
+ vector<int>& cluster_membership = *get_mutable_cluster_membership();
+ cluster_membership.resize(num_camera_blocks_);
+ for (int i = 0; i < num_camera_blocks_; ++i) {
+ cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
+ }
+ *get_mutable_num_clusters() = kNumClusters;
+
+ // Spanning forest has structure 0-1 2
+ HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
+ cluster_pairs.clear();
+ for (int i = 0; i < kNumClusters; ++i) {
+ cluster_pairs.insert(make_pair(i, i));
+ }
+ cluster_pairs.insert(make_pair(0, 1));
+
+ EXPECT_TRUE(IsSparsityStructureValid());
+ EXPECT_TRUE(PreconditionerValuesMatch());
+}
+#endif // CERES_DONT_HAVE_PROTOCOL_BUFFERS
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_NO_SUITESPARSE
