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
39 namespace ceres {
40 namespace internal {
41
ExpectMatricesAreEqual(const CompressedRowSparseMatrix & expected,const CompressedRowSparseMatrix & actual,const double tolerance)42 void 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
TEST(IncompleteQRFactorization,OneByOneMatrix)57 TEST(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
TEST(IncompleteLQFactorization,CompleteFactorization)69 TEST(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
TEST(IncompleteLQFactorization,DropEntriesAndAddRow)120 TEST(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