1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2013 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 4// 5// Redistribution and use in source and binary forms, with or without 6// modification, are permitted provided that the following conditions are met: 7// 8// * Redistributions of source code must retain the above copyright notice, 9// this list of conditions and the following disclaimer. 10// * Redistributions in binary form must reproduce the above copyright notice, 11// this list of conditions and the following disclaimer in the documentation 12// and/or other materials provided with the distribution. 13// * Neither the name of Google Inc. nor the names of its contributors may be 14// used to endorse or promote products derived from this software without 15// specific prior written permission. 16// 17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27// POSSIBILITY OF SUCH DAMAGE. 28// 29// Author: sameeragarwal@google.com (Sameer Agarwal) 30 31#include "ceres/incomplete_lq_factorization.h" 32 33#include "Eigen/Dense" 34#include "ceres/compressed_row_sparse_matrix.h" 35#include "ceres/internal/scoped_ptr.h" 36#include "glog/logging.h" 37#include "gtest/gtest.h" 38 39namespace ceres { 40namespace internal { 41 42void ExpectMatricesAreEqual(const CompressedRowSparseMatrix& expected, 43 const CompressedRowSparseMatrix& actual, 44 const double tolerance) { 45 EXPECT_EQ(expected.num_rows(), actual.num_rows()); 46 EXPECT_EQ(expected.num_cols(), actual.num_cols()); 47 for (int i = 0; i < expected.num_rows(); ++i) { 48 EXPECT_EQ(expected.rows()[i], actual.rows()[i]); 49 } 50 51 for (int i = 0; i < actual.num_nonzeros(); ++i) { 52 EXPECT_EQ(expected.cols()[i], actual.cols()[i]); 53 EXPECT_NEAR(expected.values()[i], actual.values()[i], tolerance); 54 } 55} 56 57TEST(IncompleteQRFactorization, OneByOneMatrix) { 58 CompressedRowSparseMatrix matrix(1, 1, 1); 59 matrix.mutable_rows()[0] = 0; 60 matrix.mutable_rows()[1] = 1; 61 matrix.mutable_cols()[0] = 0; 62 matrix.mutable_values()[0] = 2; 63 64 scoped_ptr<CompressedRowSparseMatrix> l( 65 IncompleteLQFactorization(matrix, 1, 0.0, 1, 0.0)); 66 ExpectMatricesAreEqual(matrix, *l, 1e-16); 67} 68 69TEST(IncompleteLQFactorization, CompleteFactorization) { 70 double dense_matrix[] = { 71 0.00000, 0.00000, 0.20522, 0.00000, 0.49077, 0.92835, 0.00000, 0.83825, 0.00000, 0.00000, // NOLINT 72 0.00000, 0.00000, 0.00000, 0.62491, 0.38144, 0.00000, 0.79394, 0.79178, 0.00000, 0.44382, // NOLINT 73 0.00000, 0.00000, 0.00000, 0.61517, 0.55941, 0.00000, 0.00000, 0.00000, 0.00000, 0.60664, // NOLINT 74 0.00000, 0.00000, 0.00000, 0.00000, 0.45031, 0.00000, 0.64132, 0.00000, 0.38832, 0.00000, // NOLINT 75 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.57121, 0.00000, 0.01375, 0.70640, 0.00000, // NOLINT 76 0.00000, 0.00000, 0.07451, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, // NOLINT 77 0.68095, 0.00000, 0.00000, 0.95473, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, // NOLINT 78 0.00000, 0.00000, 0.00000, 0.00000, 0.59374, 0.00000, 0.00000, 0.00000, 0.49139, 0.00000, // NOLINT 79 0.91276, 0.96641, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.91797, // NOLINT 80 0.96828, 0.00000, 0.00000, 0.72583, 0.00000, 0.00000, 0.81459, 0.00000, 0.04560, 0.00000 // NOLINT 81 }; 82 83 CompressedRowSparseMatrix matrix(10, 10, 100); 84 int* rows = matrix.mutable_rows(); 85 int* cols = matrix.mutable_cols(); 86 double* values = matrix.mutable_values(); 87 88 int idx = 0; 89 for (int i = 0; i < 10; ++i) { 90 rows[i] = idx; 91 for (int j = 0; j < 10; ++j) { 92 const double v = dense_matrix[i * 10 + j]; 93 if (fabs(v) > 1e-6) { 94 cols[idx] = j; 95 values[idx] = v; 96 ++idx; 97 } 98 } 99 } 100 rows[10] = idx; 101 102 scoped_ptr<CompressedRowSparseMatrix> lmatrix( 103 IncompleteLQFactorization(matrix, 10, 0.0, 10, 0.0)); 104 105 ConstMatrixRef mref(dense_matrix, 10, 10); 106 107 // Use Cholesky factorization to compute the L matrix. 108 Matrix expected_l_matrix = (mref * mref.transpose()).llt().matrixL(); 109 Matrix actual_l_matrix; 110 lmatrix->ToDenseMatrix(&actual_l_matrix); 111 112 EXPECT_NEAR((expected_l_matrix * expected_l_matrix.transpose() - 113 actual_l_matrix * actual_l_matrix.transpose()).norm(), 114 0.0, 115 1e-10) 116 << "expected: \n" << expected_l_matrix 117 << "\actual: \n" << actual_l_matrix; 118} 119 120TEST(IncompleteLQFactorization, DropEntriesAndAddRow) { 121 // Allocate space and then make it a zero sized matrix. 122 CompressedRowSparseMatrix matrix(10, 10, 100); 123 matrix.set_num_rows(0); 124 125 vector<pair<int, double> > scratch(10); 126 127 Vector dense_vector(10); 128 dense_vector(0) = 5; 129 dense_vector(1) = 1; 130 dense_vector(2) = 2; 131 dense_vector(3) = 3; 132 dense_vector(4) = 1; 133 dense_vector(5) = 4; 134 135 // Add a row with just one entry. 136 DropEntriesAndAddRow(dense_vector, 1, 1, 0, &scratch, &matrix); 137 EXPECT_EQ(matrix.num_rows(), 1); 138 EXPECT_EQ(matrix.num_cols(), 10); 139 EXPECT_EQ(matrix.num_nonzeros(), 1); 140 EXPECT_EQ(matrix.values()[0], 5.0); 141 EXPECT_EQ(matrix.cols()[0], 0); 142 143 // Add a row with six entries 144 DropEntriesAndAddRow(dense_vector, 6, 10, 0, &scratch, &matrix); 145 EXPECT_EQ(matrix.num_rows(), 2); 146 EXPECT_EQ(matrix.num_cols(), 10); 147 EXPECT_EQ(matrix.num_nonzeros(), 7); 148 for (int idx = matrix.rows()[1]; idx < matrix.rows()[2]; ++idx) { 149 EXPECT_EQ(matrix.cols()[idx], idx - matrix.rows()[1]); 150 EXPECT_EQ(matrix.values()[idx], dense_vector(idx - matrix.rows()[1])); 151 } 152 153 // Add the top 3 entries. 154 DropEntriesAndAddRow(dense_vector, 6, 3, 0, &scratch, &matrix); 155 EXPECT_EQ(matrix.num_rows(), 3); 156 EXPECT_EQ(matrix.num_cols(), 10); 157 EXPECT_EQ(matrix.num_nonzeros(), 10); 158 159 EXPECT_EQ(matrix.cols()[matrix.rows()[2]], 0); 160 EXPECT_EQ(matrix.cols()[matrix.rows()[2] + 1], 3); 161 EXPECT_EQ(matrix.cols()[matrix.rows()[2] + 2], 5); 162 163 EXPECT_EQ(matrix.values()[matrix.rows()[2]], 5); 164 EXPECT_EQ(matrix.values()[matrix.rows()[2] + 1], 3); 165 EXPECT_EQ(matrix.values()[matrix.rows()[2] + 2], 4); 166 167 // Only keep entries greater than 1.0; 168 DropEntriesAndAddRow(dense_vector, 6, 6, 0.2, &scratch, &matrix); 169 EXPECT_EQ(matrix.num_rows(), 4); 170 EXPECT_EQ(matrix.num_cols(), 10); 171 EXPECT_EQ(matrix.num_nonzeros(), 14); 172 173 EXPECT_EQ(matrix.cols()[matrix.rows()[3]], 0); 174 EXPECT_EQ(matrix.cols()[matrix.rows()[3] + 1], 2); 175 EXPECT_EQ(matrix.cols()[matrix.rows()[3] + 2], 3); 176 EXPECT_EQ(matrix.cols()[matrix.rows()[3] + 3], 5); 177 178 EXPECT_EQ(matrix.values()[matrix.rows()[3]], 5); 179 EXPECT_EQ(matrix.values()[matrix.rows()[3] + 1], 2); 180 EXPECT_EQ(matrix.values()[matrix.rows()[3] + 2], 3); 181 EXPECT_EQ(matrix.values()[matrix.rows()[3] + 3], 4); 182 183 // Only keep the top 2 entries greater than 1.0 184 DropEntriesAndAddRow(dense_vector, 6, 2, 0.2, &scratch, &matrix); 185 EXPECT_EQ(matrix.num_rows(), 5); 186 EXPECT_EQ(matrix.num_cols(), 10); 187 EXPECT_EQ(matrix.num_nonzeros(), 16); 188 189 EXPECT_EQ(matrix.cols()[matrix.rows()[4]], 0); 190 EXPECT_EQ(matrix.cols()[matrix.rows()[4] + 1], 5); 191 192 EXPECT_EQ(matrix.values()[matrix.rows()[4]], 5); 193 EXPECT_EQ(matrix.values()[matrix.rows()[4] + 1], 4); 194} 195 196 197} // namespace internal 198} // namespace ceres 199