visibility_based_preconditioner_test.cc revision 1d2624a10e2c559f8ba9ef89eaa30832c0a83a96 
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#ifndef CERES_NO_SUITESPARSE
32
33#include "ceres/visibility_based_preconditioner.h"
34
35#include "Eigen/Dense"
36#include "ceres/block_random_access_dense_matrix.h"
37#include "ceres/block_random_access_sparse_matrix.h"
38#include "ceres/block_sparse_matrix.h"
39#include "ceres/casts.h"
40#include "ceres/collections_port.h"
41#include "ceres/file.h"
42#include "ceres/internal/eigen.h"
43#include "ceres/internal/scoped_ptr.h"
44#include "ceres/linear_least_squares_problems.h"
45#include "ceres/schur_eliminator.h"
46#include "ceres/stringprintf.h"
47#include "ceres/types.h"
48#include "ceres/test_util.h"
49#include "glog/logging.h"
50#include "gtest/gtest.h"
51
52namespace ceres {
53namespace internal {
54
55// TODO(sameeragarwal): Re-enable this test once serialization is
56// working again.
57
58// using testing::AssertionResult;
59// using testing::AssertionSuccess;
60// using testing::AssertionFailure;
61
62// static const double kTolerance = 1e-12;
63
64// class VisibilityBasedPreconditionerTest : public ::testing::Test {
65//  public:
66//   static const int kCameraSize = 9;
67
68//  protected:
69//   void SetUp() {
70//     string input_file = TestFileAbsolutePath("problem-6-1384-000.lsqp");
71
72//     scoped_ptr<LinearLeastSquaresProblem> problem(
73//         CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file)));
74//     A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
75//     b_.reset(problem->b.release());
76//     D_.reset(problem->D.release());
77
78//     const CompressedRowBlockStructure* bs =
79//         CHECK_NOTNULL(A_->block_structure());
80//     const int num_col_blocks = bs->cols.size();
81
82//     num_cols_ = A_->num_cols();
83//     num_rows_ = A_->num_rows();
84//     num_eliminate_blocks_ = problem->num_eliminate_blocks;
85//     num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_;
86//     options_.elimination_groups.push_back(num_eliminate_blocks_);
87//     options_.elimination_groups.push_back(
88//         A_->block_structure()->cols.size() - num_eliminate_blocks_);
89
90//     vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
91//     for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
92//       blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
93//     }
94
95//     // The input matrix is a real jacobian and fairly poorly
96//     // conditioned. Setting D to a large constant makes the normal
97//     // equations better conditioned and makes the tests below better
98//     // conditioned.
99//     VectorRef(D_.get(), num_cols_).setConstant(10.0);
100
101//     schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks));
102//     Vector rhs(schur_complement_->num_rows());
103
104//     scoped_ptr<SchurEliminatorBase> eliminator;
105//     LinearSolver::Options eliminator_options;
106//     eliminator_options.elimination_groups = options_.elimination_groups;
107//     eliminator_options.num_threads = options_.num_threads;
108
109//     eliminator.reset(SchurEliminatorBase::Create(eliminator_options));
110//     eliminator->Init(num_eliminate_blocks_, bs);
111//     eliminator->Eliminate(A_.get(), b_.get(), D_.get(),
112//                           schur_complement_.get(), rhs.data());
113//   }
114
115
116//   AssertionResult IsSparsityStructureValid() {
117//     preconditioner_->InitStorage(*A_->block_structure());
118//     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
119//     const vector<int>& cluster_membership = get_cluster_membership();
120
121//     for (int i = 0; i < num_camera_blocks_; ++i) {
122//       for (int j = i; j < num_camera_blocks_; ++j) {
123//         if (cluster_pairs.count(make_pair(cluster_membership[i],
124//                                           cluster_membership[j]))) {
125//           if (!IsBlockPairInPreconditioner(i, j)) {
126//             return AssertionFailure()
127//                 << "block pair (" << i << "," << j << "missing";
128//           }
129//         } else {
130//           if (IsBlockPairInPreconditioner(i, j)) {
131//             return AssertionFailure()
132//                << "block pair (" << i << "," << j << "should not be present";
133//           }
134//         }
135//       }
136//     }
137//     return AssertionSuccess();
138//   }
139
140//   AssertionResult PreconditionerValuesMatch() {
141//     preconditioner_->Update(*A_, D_.get());
142//     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
143//     const BlockRandomAccessSparseMatrix* m = get_m();
144//     Matrix preconditioner_matrix;
145//     m->matrix()->ToDenseMatrix(&preconditioner_matrix);
146//     ConstMatrixRef full_schur_complement(schur_complement_->values(),
147//                                          m->num_rows(),
148//                                          m->num_rows());
149//     const int num_clusters = get_num_clusters();
150//     const int kDiagonalBlockSize =
151//         kCameraSize * num_camera_blocks_ / num_clusters;
152
153//     for (int i = 0; i < num_clusters; ++i) {
154//       for (int j = i; j < num_clusters; ++j) {
155//         double diff = 0.0;
156//         if (cluster_pairs.count(make_pair(i, j))) {
157//           diff =
158//               (preconditioner_matrix.block(kDiagonalBlockSize * i,
159//                                            kDiagonalBlockSize * j,
160//                                            kDiagonalBlockSize,
161//                                            kDiagonalBlockSize) -
162//                full_schur_complement.block(kDiagonalBlockSize * i,
163//                                            kDiagonalBlockSize * j,
164//                                            kDiagonalBlockSize,
165//                                            kDiagonalBlockSize)).norm();
166//         } else {
167//           diff = preconditioner_matrix.block(kDiagonalBlockSize * i,
168//                                              kDiagonalBlockSize * j,
169//                                              kDiagonalBlockSize,
170//                                              kDiagonalBlockSize).norm();
171//         }
172//         if (diff > kTolerance) {
173//           return AssertionFailure()
174//               << "Preconditioner block " << i << " " << j << " differs "
175//               << "from expected value by " << diff;
176//         }
177//       }
178//     }
179//     return AssertionSuccess();
180//   }
181
182//   // Accessors
183//   int get_num_blocks() { return preconditioner_->num_blocks_; }
184
185//   int get_num_clusters() { return preconditioner_->num_clusters_; }
186//   int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; }
187
188//   const vector<int>& get_block_size() {
189//     return preconditioner_->block_size_; }
190
191//   vector<int>* get_mutable_block_size() {
192//     return &preconditioner_->block_size_; }
193
194//   const vector<int>& get_cluster_membership() {
195//     return preconditioner_->cluster_membership_;
196//   }
197
198//   vector<int>* get_mutable_cluster_membership() {
199//     return &preconditioner_->cluster_membership_;
200//   }
201
202//   const set<pair<int, int> >& get_block_pairs() {
203//     return preconditioner_->block_pairs_;
204//   }
205
206//   set<pair<int, int> >* get_mutable_block_pairs() {
207//     return &preconditioner_->block_pairs_;
208//   }
209
210//   const HashSet<pair<int, int> >& get_cluster_pairs() {
211//     return preconditioner_->cluster_pairs_;
212//   }
213
214//   HashSet<pair<int, int> >* get_mutable_cluster_pairs() {
215//     return &preconditioner_->cluster_pairs_;
216//   }
217
218//   bool IsBlockPairInPreconditioner(const int block1, const int block2) {
219//     return preconditioner_->IsBlockPairInPreconditioner(block1, block2);
220//   }
221
222//   bool IsBlockPairOffDiagonal(const int block1, const int block2) {
223//     return preconditioner_->IsBlockPairOffDiagonal(block1, block2);
224//   }
225
226//   const BlockRandomAccessSparseMatrix* get_m() {
227//     return preconditioner_->m_.get();
228//   }
229
230//   int num_rows_;
231//   int num_cols_;
232//   int num_eliminate_blocks_;
233//   int num_camera_blocks_;
234
235//   scoped_ptr<BlockSparseMatrix> A_;
236//   scoped_array<double> b_;
237//   scoped_array<double> D_;
238
239//   Preconditioner::Options options_;
240//   scoped_ptr<VisibilityBasedPreconditioner> preconditioner_;
241//   scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_;
242// };
243
244// TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
245//   options_.type = CLUSTER_JACOBI;
246//   preconditioner_.reset(
247//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
248
249//   // Override the clustering to be a single clustering containing all
250//   // the cameras.
251//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
252//   for (int i = 0; i < num_camera_blocks_; ++i) {
253//     cluster_membership[i] = 0;
254//   }
255
256//   *get_mutable_num_clusters() = 1;
257
258//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
259//   cluster_pairs.clear();
260//   cluster_pairs.insert(make_pair(0, 0));
261
262//   EXPECT_TRUE(IsSparsityStructureValid());
263//   EXPECT_TRUE(PreconditionerValuesMatch());
264
265//   // Multiplication by the inverse of the preconditioner.
266//   const int num_rows = schur_complement_->num_rows();
267//   ConstMatrixRef full_schur_complement(schur_complement_->values(),
268//                                        num_rows,
269//                                        num_rows);
270//   Vector x(num_rows);
271//   Vector y(num_rows);
272//   Vector z(num_rows);
273
274//   for (int i = 0; i < num_rows; ++i) {
275//     x.setZero();
276//     y.setZero();
277//     z.setZero();
278//     x[i] = 1.0;
279//     preconditioner_->RightMultiply(x.data(), y.data());
280//     z = full_schur_complement
281//         .selfadjointView<Eigen::Upper>()
282//         .ldlt().solve(x);
283//     double max_relative_difference =
284//         ((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>();
285//     EXPECT_NEAR(max_relative_difference, 0.0, kTolerance);
286//   }
287// }
288
289
290
291// TEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) {
292//   options_.type = CLUSTER_JACOBI;
293//   preconditioner_.reset(
294//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
295
296//   // Override the clustering to be equal number of cameras.
297//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
298//   cluster_membership.resize(num_camera_blocks_);
299//   static const int kNumClusters = 3;
300
301//   for (int i = 0; i < num_camera_blocks_; ++i) {
302//     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
303//   }
304//   *get_mutable_num_clusters() = kNumClusters;
305
306//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
307//   cluster_pairs.clear();
308//   for (int i = 0; i < kNumClusters; ++i) {
309//     cluster_pairs.insert(make_pair(i, i));
310//   }
311
312//   EXPECT_TRUE(IsSparsityStructureValid());
313//   EXPECT_TRUE(PreconditionerValuesMatch());
314// }
315
316
317// TEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) {
318//   options_.type = CLUSTER_TRIDIAGONAL;
319//   preconditioner_.reset(
320//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
321//   static const int kNumClusters = 3;
322
323//   // Override the clustering to be 3 clusters.
324//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
325//   cluster_membership.resize(num_camera_blocks_);
326//   for (int i = 0; i < num_camera_blocks_; ++i) {
327//     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
328//   }
329//   *get_mutable_num_clusters() = kNumClusters;
330
331//   // Spanning forest has structure 0-1 2
332//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
333//   cluster_pairs.clear();
334//   for (int i = 0; i < kNumClusters; ++i) {
335//     cluster_pairs.insert(make_pair(i, i));
336//   }
337//   cluster_pairs.insert(make_pair(0, 1));
338
339//   EXPECT_TRUE(IsSparsityStructureValid());
340//   EXPECT_TRUE(PreconditionerValuesMatch());
341// }
342
343}  // namespace internal
344}  // namespace ceres
345
346#endif  // CERES_NO_SUITESPARSE
347