// 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 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 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 > 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