1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2010, 2011, 2012 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/implicit_schur_complement.h"
32
33 #include <cstddef>
34 #include "Eigen/Dense"
35 #include "ceres/block_random_access_dense_matrix.h"
36 #include "ceres/block_sparse_matrix.h"
37 #include "ceres/casts.h"
38 #include "ceres/internal/eigen.h"
39 #include "ceres/internal/scoped_ptr.h"
40 #include "ceres/linear_least_squares_problems.h"
41 #include "ceres/linear_solver.h"
42 #include "ceres/schur_eliminator.h"
43 #include "ceres/triplet_sparse_matrix.h"
44 #include "ceres/types.h"
45 #include "glog/logging.h"
46 #include "gtest/gtest.h"
47
48 namespace ceres {
49 namespace internal {
50
51 using testing::AssertionResult;
52
53 const double kEpsilon = 1e-14;
54
55 class ImplicitSchurComplementTest : public ::testing::Test {
56 protected :
SetUp()57 virtual void SetUp() {
58 scoped_ptr<LinearLeastSquaresProblem> problem(
59 CreateLinearLeastSquaresProblemFromId(2));
60
61 CHECK_NOTNULL(problem.get());
62 A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
63 b_.reset(problem->b.release());
64 D_.reset(problem->D.release());
65
66 num_cols_ = A_->num_cols();
67 num_rows_ = A_->num_rows();
68 num_eliminate_blocks_ = problem->num_eliminate_blocks;
69 }
70
ReducedLinearSystemAndSolution(double * D,Matrix * lhs,Vector * rhs,Vector * solution)71 void ReducedLinearSystemAndSolution(double* D,
72 Matrix* lhs,
73 Vector* rhs,
74 Vector* solution) {
75 const CompressedRowBlockStructure* bs = A_->block_structure();
76 const int num_col_blocks = bs->cols.size();
77 vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
78 for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
79 blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
80 }
81
82 BlockRandomAccessDenseMatrix blhs(blocks);
83 const int num_schur_rows = blhs.num_rows();
84
85 LinearSolver::Options options;
86 options.elimination_groups.push_back(num_eliminate_blocks_);
87 options.type = DENSE_SCHUR;
88
89 scoped_ptr<SchurEliminatorBase> eliminator(
90 SchurEliminatorBase::Create(options));
91 CHECK_NOTNULL(eliminator.get());
92 eliminator->Init(num_eliminate_blocks_, bs);
93
94 lhs->resize(num_schur_rows, num_schur_rows);
95 rhs->resize(num_schur_rows);
96
97 eliminator->Eliminate(A_.get(), b_.get(), D, &blhs, rhs->data());
98
99 MatrixRef lhs_ref(blhs.mutable_values(), num_schur_rows, num_schur_rows);
100
101 // lhs_ref is an upper triangular matrix. Construct a full version
102 // of lhs_ref in lhs by transposing lhs_ref, choosing the strictly
103 // lower triangular part of the matrix and adding it to lhs_ref.
104 *lhs = lhs_ref;
105 lhs->triangularView<Eigen::StrictlyLower>() =
106 lhs_ref.triangularView<Eigen::StrictlyUpper>().transpose();
107
108 solution->resize(num_cols_);
109 solution->setZero();
110 VectorRef schur_solution(solution->data() + num_cols_ - num_schur_rows,
111 num_schur_rows);
112 schur_solution = lhs->selfadjointView<Eigen::Upper>().llt().solve(*rhs);
113 eliminator->BackSubstitute(A_.get(), b_.get(), D,
114 schur_solution.data(), solution->data());
115 }
116
TestImplicitSchurComplement(double * D)117 AssertionResult TestImplicitSchurComplement(double* D) {
118 Matrix lhs;
119 Vector rhs;
120 Vector reference_solution;
121 ReducedLinearSystemAndSolution(D, &lhs, &rhs, &reference_solution);
122
123 LinearSolver::Options options;
124 options.elimination_groups.push_back(num_eliminate_blocks_);
125 options.preconditioner_type = JACOBI;
126 ImplicitSchurComplement isc(options);
127 isc.Init(*A_, D, b_.get());
128
129 int num_sc_cols = lhs.cols();
130
131 for (int i = 0; i < num_sc_cols; ++i) {
132 Vector x(num_sc_cols);
133 x.setZero();
134 x(i) = 1.0;
135
136 Vector y(num_sc_cols);
137 y = lhs * x;
138
139 Vector z(num_sc_cols);
140 isc.RightMultiply(x.data(), z.data());
141
142 // The i^th column of the implicit schur complement is the same as
143 // the explicit schur complement.
144 if ((y - z).norm() > kEpsilon) {
145 return testing::AssertionFailure()
146 << "Explicit and Implicit SchurComplements differ in "
147 << "column " << i << ". explicit: " << y.transpose()
148 << " implicit: " << z.transpose();
149 }
150 }
151
152 // Compare the rhs of the reduced linear system
153 if ((isc.rhs() - rhs).norm() > kEpsilon) {
154 return testing::AssertionFailure()
155 << "Explicit and Implicit SchurComplements differ in "
156 << "rhs. explicit: " << rhs.transpose()
157 << " implicit: " << isc.rhs().transpose();
158 }
159
160 // Reference solution to the f_block.
161 const Vector reference_f_sol =
162 lhs.selfadjointView<Eigen::Upper>().llt().solve(rhs);
163
164 // Backsubstituted solution from the implicit schur solver using the
165 // reference solution to the f_block.
166 Vector sol(num_cols_);
167 isc.BackSubstitute(reference_f_sol.data(), sol.data());
168 if ((sol - reference_solution).norm() > kEpsilon) {
169 return testing::AssertionFailure()
170 << "Explicit and Implicit SchurComplements solutions differ. "
171 << "explicit: " << reference_solution.transpose()
172 << " implicit: " << sol.transpose();
173 }
174
175 return testing::AssertionSuccess();
176 }
177
178 int num_rows_;
179 int num_cols_;
180 int num_eliminate_blocks_;
181
182 scoped_ptr<BlockSparseMatrix> A_;
183 scoped_array<double> b_;
184 scoped_array<double> D_;
185 };
186
187 // Verify that the Schur Complement matrix implied by the
188 // ImplicitSchurComplement class matches the one explicitly computed
189 // by the SchurComplement solver.
190 //
191 // We do this with and without regularization to check that the
192 // support for the LM diagonal is correct.
TEST_F(ImplicitSchurComplementTest,SchurMatrixValuesTest)193 TEST_F(ImplicitSchurComplementTest, SchurMatrixValuesTest) {
194 EXPECT_TRUE(TestImplicitSchurComplement(NULL));
195 EXPECT_TRUE(TestImplicitSchurComplement(D_.get()));
196 }
197
198 } // namespace internal
199 } // namespace ceres
200