schur_complement_solver_test.cc 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322
  1. // Ceres Solver - A fast non-linear least squares minimizer
  2. // Copyright 2015 Google Inc. All rights reserved.
  3. // http://ceres-solver.org/
  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. #include "ceres/schur_complement_solver.h"
  31. #include <cstddef>
  32. #include <memory>
  33. #include "ceres/block_sparse_matrix.h"
  34. #include "ceres/block_structure.h"
  35. #include "ceres/casts.h"
  36. #include "ceres/context_impl.h"
  37. #include "ceres/detect_structure.h"
  38. #include "ceres/linear_least_squares_problems.h"
  39. #include "ceres/linear_solver.h"
  40. #include "ceres/triplet_sparse_matrix.h"
  41. #include "ceres/types.h"
  42. #include "glog/logging.h"
  43. #include "gtest/gtest.h"
  44. namespace ceres::internal {
  45. class SchurComplementSolverTest : public ::testing::Test {
  46. protected:
  47. void SetUpFromProblemId(int problem_id) {
  48. std::unique_ptr<LinearLeastSquaresProblem> problem =
  49. CreateLinearLeastSquaresProblemFromId(problem_id);
  50. CHECK(problem != nullptr);
  51. A.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
  52. b = std::move(problem->b);
  53. D = std::move(problem->D);
  54. num_cols = A->num_cols();
  55. num_rows = A->num_rows();
  56. num_eliminate_blocks = problem->num_eliminate_blocks;
  57. x.resize(num_cols);
  58. sol.resize(num_cols);
  59. sol_d.resize(num_cols);
  60. LinearSolver::Options options;
  61. options.type = DENSE_QR;
  62. ContextImpl context;
  63. options.context = &context;
  64. std::unique_ptr<LinearSolver> qr(LinearSolver::Create(options));
  65. TripletSparseMatrix triplet_A(
  66. A->num_rows(), A->num_cols(), A->num_nonzeros());
  67. A->ToTripletSparseMatrix(&triplet_A);
  68. // Gold standard solutions using dense QR factorization.
  69. DenseSparseMatrix dense_A(triplet_A);
  70. qr->Solve(&dense_A, b.get(), LinearSolver::PerSolveOptions(), sol.data());
  71. // Gold standard solution with appended diagonal.
  72. LinearSolver::PerSolveOptions per_solve_options;
  73. per_solve_options.D = D.get();
  74. qr->Solve(&dense_A, b.get(), per_solve_options, sol_d.data());
  75. }
  76. void ComputeAndCompareSolutions(
  77. int problem_id,
  78. bool regularization,
  79. ceres::LinearSolverType linear_solver_type,
  80. ceres::DenseLinearAlgebraLibraryType dense_linear_algebra_library_type,
  81. ceres::SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
  82. ceres::internal::OrderingType ordering_type) {
  83. SetUpFromProblemId(problem_id);
  84. LinearSolver::Options options;
  85. options.elimination_groups.push_back(num_eliminate_blocks);
  86. options.elimination_groups.push_back(A->block_structure()->cols.size() -
  87. num_eliminate_blocks);
  88. options.type = linear_solver_type;
  89. options.dense_linear_algebra_library_type =
  90. dense_linear_algebra_library_type;
  91. options.sparse_linear_algebra_library_type =
  92. sparse_linear_algebra_library_type;
  93. options.ordering_type = ordering_type;
  94. ContextImpl context;
  95. options.context = &context;
  96. DetectStructure(*A->block_structure(),
  97. num_eliminate_blocks,
  98. &options.row_block_size,
  99. &options.e_block_size,
  100. &options.f_block_size);
  101. std::unique_ptr<LinearSolver> solver(LinearSolver::Create(options));
  102. LinearSolver::PerSolveOptions per_solve_options;
  103. LinearSolver::Summary summary;
  104. if (regularization) {
  105. per_solve_options.D = D.get();
  106. }
  107. summary = solver->Solve(A.get(), b.get(), per_solve_options, x.data());
  108. EXPECT_EQ(summary.termination_type, LinearSolverTerminationType::SUCCESS);
  109. if (regularization) {
  110. ASSERT_NEAR((sol_d - x).norm() / num_cols, 0, 1e-10)
  111. << "Regularized Expected solution: " << sol_d.transpose()
  112. << " Actual solution: " << x.transpose();
  113. } else {
  114. ASSERT_NEAR((sol - x).norm() / num_cols, 0, 1e-10)
  115. << "Unregularized Expected solution: " << sol.transpose()
  116. << " Actual solution: " << x.transpose();
  117. }
  118. }
  119. int num_rows;
  120. int num_cols;
  121. int num_eliminate_blocks;
  122. std::unique_ptr<BlockSparseMatrix> A;
  123. std::unique_ptr<double[]> b;
  124. std::unique_ptr<double[]> D;
  125. Vector x;
  126. Vector sol;
  127. Vector sol_d;
  128. };
  129. // TODO(sameeragarwal): Refactor these using value parameterized tests.
  130. // TODO(sameeragarwal): More extensive tests using random matrices.
  131. TEST_F(SchurComplementSolverTest, DenseSchurWithEigenSmallProblem) {
  132. ComputeAndCompareSolutions(
  133. 2, false, DENSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  134. ComputeAndCompareSolutions(
  135. 2, true, DENSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  136. }
  137. TEST_F(SchurComplementSolverTest, DenseSchurWithEigenLargeProblem) {
  138. ComputeAndCompareSolutions(
  139. 3, false, DENSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  140. ComputeAndCompareSolutions(
  141. 3, true, DENSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  142. }
  143. TEST_F(SchurComplementSolverTest, DenseSchurWithEigenVaryingFBlockSize) {
  144. ComputeAndCompareSolutions(
  145. 4, true, DENSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  146. }
  147. #ifndef CERES_NO_LAPACK
  148. TEST_F(SchurComplementSolverTest, DenseSchurWithLAPACKSmallProblem) {
  149. ComputeAndCompareSolutions(
  150. 2, false, DENSE_SCHUR, LAPACK, SUITE_SPARSE, OrderingType::NATURAL);
  151. ComputeAndCompareSolutions(
  152. 2, true, DENSE_SCHUR, LAPACK, SUITE_SPARSE, OrderingType::NATURAL);
  153. }
  154. TEST_F(SchurComplementSolverTest, DenseSchurWithLAPACKLargeProblem) {
  155. ComputeAndCompareSolutions(
  156. 3, false, DENSE_SCHUR, LAPACK, SUITE_SPARSE, OrderingType::NATURAL);
  157. ComputeAndCompareSolutions(
  158. 3, true, DENSE_SCHUR, LAPACK, SUITE_SPARSE, OrderingType::NATURAL);
  159. }
  160. #endif
  161. #ifndef CERES_NO_SUITESPARSE
  162. TEST_F(SchurComplementSolverTest,
  163. SparseSchurWithSuiteSparseSmallProblemNATURAL) {
  164. ComputeAndCompareSolutions(
  165. 2, false, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  166. ComputeAndCompareSolutions(
  167. 2, true, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  168. }
  169. TEST_F(SchurComplementSolverTest,
  170. SparseSchurWithSuiteSparseLargeProblemNATURAL) {
  171. ComputeAndCompareSolutions(
  172. 3, false, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  173. ComputeAndCompareSolutions(
  174. 3, true, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NATURAL);
  175. }
  176. TEST_F(SchurComplementSolverTest, SparseSchurWithSuiteSparseSmallProblemAMD) {
  177. ComputeAndCompareSolutions(
  178. 2, false, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::AMD);
  179. ComputeAndCompareSolutions(
  180. 2, true, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::AMD);
  181. }
  182. TEST_F(SchurComplementSolverTest, SparseSchurWithSuiteSparseLargeProblemAMD) {
  183. ComputeAndCompareSolutions(
  184. 3, false, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::AMD);
  185. ComputeAndCompareSolutions(
  186. 3, true, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::AMD);
  187. }
  188. #ifndef CERES_NO_EIGEN_METIS
  189. TEST_F(SchurComplementSolverTest,
  190. SparseSchurWithSuiteSparseSmallProblemNESDIS) {
  191. ComputeAndCompareSolutions(
  192. 2, false, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NESDIS);
  193. ComputeAndCompareSolutions(
  194. 2, true, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NESDIS);
  195. }
  196. TEST_F(SchurComplementSolverTest,
  197. SparseSchurWithSuiteSparseLargeProblemNESDIS) {
  198. ComputeAndCompareSolutions(
  199. 3, false, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NESDIS);
  200. ComputeAndCompareSolutions(
  201. 3, true, SPARSE_SCHUR, EIGEN, SUITE_SPARSE, OrderingType::NESDIS);
  202. }
  203. #endif // CERES_NO_EIGEN_METIS
  204. #endif // CERES_NO_SUITESPARSE
  205. #ifndef CERES_NO_ACCELERATE_SPARSE
  206. TEST_F(SchurComplementSolverTest,
  207. SparseSchurWithAccelerateSparseSmallProblemAMD) {
  208. ComputeAndCompareSolutions(
  209. 2, false, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::AMD);
  210. ComputeAndCompareSolutions(
  211. 2, true, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::AMD);
  212. }
  213. TEST_F(SchurComplementSolverTest,
  214. SparseSchurWithAccelerateSparseSmallProblemNESDIS) {
  215. ComputeAndCompareSolutions(
  216. 2, false, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::NESDIS);
  217. ComputeAndCompareSolutions(
  218. 2, true, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::NESDIS);
  219. }
  220. TEST_F(SchurComplementSolverTest,
  221. SparseSchurWithAccelerateSparseLargeProblemAMD) {
  222. ComputeAndCompareSolutions(
  223. 3, false, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::AMD);
  224. ComputeAndCompareSolutions(
  225. 3, true, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::AMD);
  226. }
  227. TEST_F(SchurComplementSolverTest,
  228. SparseSchurWithAccelerateSparseLargeProblemNESDIS) {
  229. ComputeAndCompareSolutions(
  230. 3, false, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::NESDIS);
  231. ComputeAndCompareSolutions(
  232. 3, true, SPARSE_SCHUR, EIGEN, ACCELERATE_SPARSE, OrderingType::NESDIS);
  233. }
  234. #endif // CERES_NO_ACCELERATE_SPARSE
  235. #ifdef CERES_USE_EIGEN_SPARSE
  236. TEST_F(SchurComplementSolverTest, SparseSchurWithEigenSparseSmallProblemAMD) {
  237. ComputeAndCompareSolutions(
  238. 2, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::AMD);
  239. ComputeAndCompareSolutions(
  240. 2, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::AMD);
  241. }
  242. #ifndef CERES_NO_EIGEN_METIS
  243. TEST_F(SchurComplementSolverTest,
  244. SparseSchurWithEigenSparseSmallProblemNESDIS) {
  245. ComputeAndCompareSolutions(
  246. 2, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NESDIS);
  247. ComputeAndCompareSolutions(
  248. 2, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NESDIS);
  249. }
  250. #endif
  251. TEST_F(SchurComplementSolverTest,
  252. SparseSchurWithEigenSparseSmallProblemNATURAL) {
  253. ComputeAndCompareSolutions(
  254. 2, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NATURAL);
  255. ComputeAndCompareSolutions(
  256. 2, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NATURAL);
  257. }
  258. TEST_F(SchurComplementSolverTest, SparseSchurWithEigenSparseLargeProblemAMD) {
  259. ComputeAndCompareSolutions(
  260. 3, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::AMD);
  261. ComputeAndCompareSolutions(
  262. 3, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::AMD);
  263. }
  264. #ifndef CERES_NO_EIGEN_METIS
  265. TEST_F(SchurComplementSolverTest,
  266. SparseSchurWithEigenSparseLargeProblemNESDIS) {
  267. ComputeAndCompareSolutions(
  268. 3, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NESDIS);
  269. ComputeAndCompareSolutions(
  270. 3, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NESDIS);
  271. }
  272. #endif
  273. TEST_F(SchurComplementSolverTest,
  274. SparseSchurWithEigenSparseLargeProblemNATURAL) {
  275. ComputeAndCompareSolutions(
  276. 3, false, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NATURAL);
  277. ComputeAndCompareSolutions(
  278. 3, true, SPARSE_SCHUR, EIGEN, EIGEN_SPARSE, OrderingType::NATURAL);
  279. }
  280. #endif // CERES_USE_EIGEN_SPARSE
  281. } // namespace ceres::internal