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