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 (at) google.com (Sameer Agarwal) 30 31 #include "ceres/schur_eliminator.h" 32 33 #include "Eigen/Dense" 34 #include "ceres/block_random_access_dense_matrix.h" 35 #include "ceres/block_sparse_matrix.h" 36 #include "ceres/casts.h" 37 #include "ceres/detect_structure.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/test_util.h" 42 #include "ceres/triplet_sparse_matrix.h" 43 #include "ceres/types.h" 44 #include "glog/logging.h" 45 #include "gtest/gtest.h" 46 47 // TODO(sameeragarwal): Reduce the size of these tests and redo the 48 // parameterization to be more efficient. 49 50 namespace ceres { 51 namespace internal { 52 53 class SchurEliminatorTest : public ::testing::Test { 54 protected: 55 void SetUpFromId(int id) { 56 scoped_ptr<LinearLeastSquaresProblem> 57 problem(CreateLinearLeastSquaresProblemFromId(id)); 58 CHECK_NOTNULL(problem.get()); 59 SetupHelper(problem.get()); 60 } 61 62 void SetupHelper(LinearLeastSquaresProblem* problem) { 63 A.reset(down_cast<BlockSparseMatrix*>(problem->A.release())); 64 b.reset(problem->b.release()); 65 D.reset(problem->D.release()); 66 67 num_eliminate_blocks = problem->num_eliminate_blocks; 68 num_eliminate_cols = 0; 69 const CompressedRowBlockStructure* bs = A->block_structure(); 70 71 for (int i = 0; i < num_eliminate_blocks; ++i) { 72 num_eliminate_cols += bs->cols[i].size; 73 } 74 } 75 76 // Compute the golden values for the reduced linear system and the 77 // solution to the linear least squares problem using dense linear 78 // algebra. 79 void ComputeReferenceSolution(const Vector& D) { 80 Matrix J; 81 A->ToDenseMatrix(&J); 82 VectorRef f(b.get(), J.rows()); 83 84 Matrix H = (D.cwiseProduct(D)).asDiagonal(); 85 H.noalias() += J.transpose() * J; 86 87 const Vector g = J.transpose() * f; 88 const int schur_size = J.cols() - num_eliminate_cols; 89 90 lhs_expected.resize(schur_size, schur_size); 91 lhs_expected.setZero(); 92 93 rhs_expected.resize(schur_size); 94 rhs_expected.setZero(); 95 96 sol_expected.resize(J.cols()); 97 sol_expected.setZero(); 98 99 Matrix P = H.block(0, 0, num_eliminate_cols, num_eliminate_cols); 100 Matrix Q = H.block(0, 101 num_eliminate_cols, 102 num_eliminate_cols, 103 schur_size); 104 Matrix R = H.block(num_eliminate_cols, 105 num_eliminate_cols, 106 schur_size, 107 schur_size); 108 int row = 0; 109 const CompressedRowBlockStructure* bs = A->block_structure(); 110 for (int i = 0; i < num_eliminate_blocks; ++i) { 111 const int block_size = bs->cols[i].size; 112 P.block(row, row, block_size, block_size) = 113 P 114 .block(row, row, block_size, block_size) 115 .llt() 116 .solve(Matrix::Identity(block_size, block_size)); 117 row += block_size; 118 } 119 120 lhs_expected 121 .triangularView<Eigen::Upper>() = R - Q.transpose() * P * Q; 122 rhs_expected = 123 g.tail(schur_size) - Q.transpose() * P * g.head(num_eliminate_cols); 124 sol_expected = H.llt().solve(g); 125 } 126 127 void EliminateSolveAndCompare(const VectorRef& diagonal, 128 bool use_static_structure, 129 const double relative_tolerance) { 130 const CompressedRowBlockStructure* bs = A->block_structure(); 131 const int num_col_blocks = bs->cols.size(); 132 vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); 133 for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) { 134 blocks[i - num_eliminate_blocks] = bs->cols[i].size; 135 } 136 137 BlockRandomAccessDenseMatrix lhs(blocks); 138 139 const int num_cols = A->num_cols(); 140 const int schur_size = lhs.num_rows(); 141 142 Vector rhs(schur_size); 143 144 LinearSolver::Options options; 145 options.elimination_groups.push_back(num_eliminate_blocks); 146 if (use_static_structure) { 147 DetectStructure(*bs, 148 num_eliminate_blocks, 149 &options.row_block_size, 150 &options.e_block_size, 151 &options.f_block_size); 152 } 153 154 scoped_ptr<SchurEliminatorBase> eliminator; 155 eliminator.reset(SchurEliminatorBase::Create(options)); 156 eliminator->Init(num_eliminate_blocks, A->block_structure()); 157 eliminator->Eliminate(A.get(), b.get(), diagonal.data(), &lhs, rhs.data()); 158 159 MatrixRef lhs_ref(lhs.mutable_values(), lhs.num_rows(), lhs.num_cols()); 160 Vector reduced_sol = 161 lhs_ref 162 .selfadjointView<Eigen::Upper>() 163 .llt() 164 .solve(rhs); 165 166 // Solution to the linear least squares problem. 167 Vector sol(num_cols); 168 sol.setZero(); 169 sol.tail(schur_size) = reduced_sol; 170 eliminator->BackSubstitute(A.get(), 171 b.get(), 172 diagonal.data(), 173 reduced_sol.data(), 174 sol.data()); 175 176 Matrix delta = (lhs_ref - lhs_expected).selfadjointView<Eigen::Upper>(); 177 double diff = delta.norm(); 178 EXPECT_NEAR(diff / lhs_expected.norm(), 0.0, relative_tolerance); 179 EXPECT_NEAR((rhs - rhs_expected).norm() / rhs_expected.norm(), 0.0, 180 relative_tolerance); 181 EXPECT_NEAR((sol - sol_expected).norm() / sol_expected.norm(), 0.0, 182 relative_tolerance); 183 } 184 185 scoped_ptr<BlockSparseMatrix> A; 186 scoped_array<double> b; 187 scoped_array<double> D; 188 int num_eliminate_blocks; 189 int num_eliminate_cols; 190 191 Matrix lhs_expected; 192 Vector rhs_expected; 193 Vector sol_expected; 194 }; 195 196 TEST_F(SchurEliminatorTest, ScalarProblem) { 197 SetUpFromId(2); 198 Vector zero(A->num_cols()); 199 zero.setZero(); 200 201 ComputeReferenceSolution(VectorRef(zero.data(), A->num_cols())); 202 EliminateSolveAndCompare(VectorRef(zero.data(), A->num_cols()), true, 1e-14); 203 EliminateSolveAndCompare(VectorRef(zero.data(), A->num_cols()), false, 1e-14); 204 205 ComputeReferenceSolution(VectorRef(D.get(), A->num_cols())); 206 EliminateSolveAndCompare(VectorRef(D.get(), A->num_cols()), true, 1e-14); 207 EliminateSolveAndCompare(VectorRef(D.get(), A->num_cols()), false, 1e-14); 208 } 209 210 } // namespace internal 211 } // namespace ceres 212
