diff options
Diffstat (limited to 'internal/ceres/incomplete_lq_factorization_test.cc')
| -rw-r--r-- | internal/ceres/incomplete_lq_factorization_test.cc | 198 |
1 files changed, 198 insertions, 0 deletions
diff --git a/internal/ceres/incomplete_lq_factorization_test.cc b/internal/ceres/incomplete_lq_factorization_test.cc new file mode 100644 index 0000000..1ed181f --- /dev/null +++ b/internal/ceres/incomplete_lq_factorization_test.cc @@ -0,0 +1,198 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// 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/incomplete_lq_factorization.h" + +#include "Eigen/Dense" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/internal/scoped_ptr.h" +#include "glog/logging.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +void ExpectMatricesAreEqual(const CompressedRowSparseMatrix& expected, + const CompressedRowSparseMatrix& actual, + const double tolerance) { + EXPECT_EQ(expected.num_rows(), actual.num_rows()); + EXPECT_EQ(expected.num_cols(), actual.num_cols()); + for (int i = 0; i < expected.num_rows(); ++i) { + EXPECT_EQ(expected.rows()[i], actual.rows()[i]); + } + + for (int i = 0; i < actual.num_nonzeros(); ++i) { + EXPECT_EQ(expected.cols()[i], actual.cols()[i]); + EXPECT_NEAR(expected.values()[i], actual.values()[i], tolerance); + } +} + +TEST(IncompleteQRFactorization, OneByOneMatrix) { + CompressedRowSparseMatrix matrix(1, 1, 1); + matrix.mutable_rows()[0] = 0; + matrix.mutable_rows()[1] = 1; + matrix.mutable_cols()[0] = 0; + matrix.mutable_values()[0] = 2; + + scoped_ptr<CompressedRowSparseMatrix> l( + IncompleteLQFactorization(matrix, 1, 0.0, 1, 0.0)); + ExpectMatricesAreEqual(matrix, *l, 1e-16); +} + +TEST(IncompleteLQFactorization, CompleteFactorization) { + double dense_matrix[] = { + 0.00000, 0.00000, 0.20522, 0.00000, 0.49077, 0.92835, 0.00000, 0.83825, 0.00000, 0.00000, // NOLINT + 0.00000, 0.00000, 0.00000, 0.62491, 0.38144, 0.00000, 0.79394, 0.79178, 0.00000, 0.44382, // NOLINT + 0.00000, 0.00000, 0.00000, 0.61517, 0.55941, 0.00000, 0.00000, 0.00000, 0.00000, 0.60664, // NOLINT + 0.00000, 0.00000, 0.00000, 0.00000, 0.45031, 0.00000, 0.64132, 0.00000, 0.38832, 0.00000, // NOLINT + 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.57121, 0.00000, 0.01375, 0.70640, 0.00000, // NOLINT + 0.00000, 0.00000, 0.07451, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, // NOLINT + 0.68095, 0.00000, 0.00000, 0.95473, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, // NOLINT + 0.00000, 0.00000, 0.00000, 0.00000, 0.59374, 0.00000, 0.00000, 0.00000, 0.49139, 0.00000, // NOLINT + 0.91276, 0.96641, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.91797, // NOLINT + 0.96828, 0.00000, 0.00000, 0.72583, 0.00000, 0.00000, 0.81459, 0.00000, 0.04560, 0.00000 // NOLINT + }; + + CompressedRowSparseMatrix matrix(10, 10, 100); + int* rows = matrix.mutable_rows(); + int* cols = matrix.mutable_cols(); + double* values = matrix.mutable_values(); + + int idx = 0; + for (int i = 0; i < 10; ++i) { + rows[i] = idx; + for (int j = 0; j < 10; ++j) { + const double v = dense_matrix[i * 10 + j]; + if (fabs(v) > 1e-6) { + cols[idx] = j; + values[idx] = v; + ++idx; + } + } + } + rows[10] = idx; + + scoped_ptr<CompressedRowSparseMatrix> lmatrix( + IncompleteLQFactorization(matrix, 10, 0.0, 10, 0.0)); + + ConstMatrixRef mref(dense_matrix, 10, 10); + + // Use Cholesky factorization to compute the L matrix. + Matrix expected_l_matrix = (mref * mref.transpose()).llt().matrixL(); + Matrix actual_l_matrix; + lmatrix->ToDenseMatrix(&actual_l_matrix); + + EXPECT_NEAR((expected_l_matrix * expected_l_matrix.transpose() - + actual_l_matrix * actual_l_matrix.transpose()).norm(), + 0.0, + 1e-10) + << "expected: \n" << expected_l_matrix + << "\actual: \n" << actual_l_matrix; +} + +TEST(IncompleteLQFactorization, DropEntriesAndAddRow) { + // Allocate space and then make it a zero sized matrix. + CompressedRowSparseMatrix matrix(10, 10, 100); + matrix.set_num_rows(0); + + vector<pair<int, double> > scratch(10); + + Vector dense_vector(10); + dense_vector(0) = 5; + dense_vector(1) = 1; + dense_vector(2) = 2; + dense_vector(3) = 3; + dense_vector(4) = 1; + dense_vector(5) = 4; + + // Add a row with just one entry. + DropEntriesAndAddRow(dense_vector, 1, 1, 0, &scratch, &matrix); + EXPECT_EQ(matrix.num_rows(), 1); + EXPECT_EQ(matrix.num_cols(), 10); + EXPECT_EQ(matrix.num_nonzeros(), 1); + EXPECT_EQ(matrix.values()[0], 5.0); + EXPECT_EQ(matrix.cols()[0], 0); + + // Add a row with six entries + DropEntriesAndAddRow(dense_vector, 6, 10, 0, &scratch, &matrix); + EXPECT_EQ(matrix.num_rows(), 2); + EXPECT_EQ(matrix.num_cols(), 10); + EXPECT_EQ(matrix.num_nonzeros(), 7); + for (int idx = matrix.rows()[1]; idx < matrix.rows()[2]; ++idx) { + EXPECT_EQ(matrix.cols()[idx], idx - matrix.rows()[1]); + EXPECT_EQ(matrix.values()[idx], dense_vector(idx - matrix.rows()[1])); + } + + // Add the top 3 entries. + DropEntriesAndAddRow(dense_vector, 6, 3, 0, &scratch, &matrix); + EXPECT_EQ(matrix.num_rows(), 3); + EXPECT_EQ(matrix.num_cols(), 10); + EXPECT_EQ(matrix.num_nonzeros(), 10); + + EXPECT_EQ(matrix.cols()[matrix.rows()[2]], 0); + EXPECT_EQ(matrix.cols()[matrix.rows()[2] + 1], 3); + EXPECT_EQ(matrix.cols()[matrix.rows()[2] + 2], 5); + + EXPECT_EQ(matrix.values()[matrix.rows()[2]], 5); + EXPECT_EQ(matrix.values()[matrix.rows()[2] + 1], 3); + EXPECT_EQ(matrix.values()[matrix.rows()[2] + 2], 4); + + // Only keep entries greater than 1.0; + DropEntriesAndAddRow(dense_vector, 6, 6, 0.2, &scratch, &matrix); + EXPECT_EQ(matrix.num_rows(), 4); + EXPECT_EQ(matrix.num_cols(), 10); + EXPECT_EQ(matrix.num_nonzeros(), 14); + + EXPECT_EQ(matrix.cols()[matrix.rows()[3]], 0); + EXPECT_EQ(matrix.cols()[matrix.rows()[3] + 1], 2); + EXPECT_EQ(matrix.cols()[matrix.rows()[3] + 2], 3); + EXPECT_EQ(matrix.cols()[matrix.rows()[3] + 3], 5); + + EXPECT_EQ(matrix.values()[matrix.rows()[3]], 5); + EXPECT_EQ(matrix.values()[matrix.rows()[3] + 1], 2); + EXPECT_EQ(matrix.values()[matrix.rows()[3] + 2], 3); + EXPECT_EQ(matrix.values()[matrix.rows()[3] + 3], 4); + + // Only keep the top 2 entries greater than 1.0 + DropEntriesAndAddRow(dense_vector, 6, 2, 0.2, &scratch, &matrix); + EXPECT_EQ(matrix.num_rows(), 5); + EXPECT_EQ(matrix.num_cols(), 10); + EXPECT_EQ(matrix.num_nonzeros(), 16); + + EXPECT_EQ(matrix.cols()[matrix.rows()[4]], 0); + EXPECT_EQ(matrix.cols()[matrix.rows()[4] + 1], 5); + + EXPECT_EQ(matrix.values()[matrix.rows()[4]], 5); + EXPECT_EQ(matrix.values()[matrix.rows()[4] + 1], 4); +} + + +} // namespace internal +} // namespace ceres |