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