1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26
27 // clang-format off
28 #include "g2o/config.h"
29 // clang-format on
30
31 #include "g2o/solvers/eigen/linear_solver_eigen.h"
32 #ifdef G2O_HAVE_CSPARSE
33 #include "g2o/solvers/csparse/linear_solver_csparse.h"
34 #endif
35 #ifdef G2O_HAVE_CHOLMOD
36 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
37 #endif
38 #include "gtest/gtest.h"
39 #include "sparse_system_helper.h"
40
41 struct BlockOrdering {
42 static constexpr bool blockOrdering = true;
43 };
44
45 struct NoBlockOrdering {
46 static constexpr bool blockOrdering = false;
47 };
48
49 /**
50 * Type parameterized class for a fixture to setup a linear solver along with some data of a linear
51 * system to be solved.
52 */
53 template <typename T>
54 class LS : public testing::Test {
55 public:
56 using LinearSolverType = typename T::first_type;
57 using OrderingType = typename T::second_type;
58
LS()59 LS() : linearsolver(g2o::make_unique<LinearSolverType>()) {}
60
61 protected:
62 std::unique_ptr<LinearSolverType> linearsolver;
63 g2o::SparseBlockMatrixX sparse_matrix = g2o::internal::createTestMatrix();
64 g2o::MatrixX matrix_inverse = g2o::internal::createTestMatrixInverse();
65 g2o::VectorX x_vector = g2o::internal::createTestVectorX();
66 g2o::VectorX b_vector = g2o::internal::createTestVectorB();
67 };
68 TYPED_TEST_SUITE_P(LS);
69
TYPED_TEST_P(LS,Solve)70 TYPED_TEST_P(LS, Solve) {
71 this->linearsolver->setBlockOrdering(TypeParam::second_type::blockOrdering);
72
73 g2o::VectorX solver_solution;
74 for (int solve_iter = 0; solve_iter < 2; ++solve_iter) {
75 solver_solution.setZero(this->b_vector.size());
76 this->linearsolver->solve(this->sparse_matrix, solver_solution.data(), this->b_vector.data());
77
78 ASSERT_TRUE(solver_solution.isApprox(this->x_vector, 1e-6))
79 << "Solution differs on iteration " << solve_iter;
80 }
81 }
82
TYPED_TEST_P(LS,SolvePattern)83 TYPED_TEST_P(LS, SolvePattern) {
84 this->linearsolver->setBlockOrdering(TypeParam::second_type::blockOrdering);
85
86 g2o::SparseBlockMatrixX spinv;
87 std::vector<std::pair<int, int> > blockIndices;
88 for (int i = 0; i < static_cast<int>(this->sparse_matrix.rowBlockIndices().size()); ++i)
89 blockIndices.emplace_back(std::make_pair(i, i));
90
91 bool state = this->linearsolver->solvePattern(spinv, blockIndices, this->sparse_matrix);
92 ASSERT_TRUE(!state || spinv.rowBlockIndices().size() == blockIndices.size());
93 if (!state) { // solver does not implement solving for a pattern return in this case
94 std::cerr << "Solver does not support solvePattern()" << std::endl;
95 SUCCEED();
96 return;
97 }
98
99 for (const auto& idx : blockIndices) {
100 int rr = spinv.rowBaseOfBlock(idx.first);
101 int cc = spinv.colBaseOfBlock(idx.second);
102 int numRows = spinv.rowsOfBlock(idx.first);
103 int numCols = spinv.colsOfBlock(idx.second);
104
105 g2o::MatrixX expected = this->matrix_inverse.block(rr, cc, numRows, numCols);
106 g2o::MatrixX actual = *spinv.block(idx.first, idx.second);
107
108 EXPECT_TRUE(actual.isApprox(expected, 1e-6))
109 << "block " << idx.first << " " << idx.second << " differs";
110 }
111 }
112
TYPED_TEST_P(LS,SolveBlocks)113 TYPED_TEST_P(LS, SolveBlocks) {
114 this->linearsolver->setBlockOrdering(TypeParam::second_type::blockOrdering);
115
116 number_t** blocks = nullptr;
117 bool state = this->linearsolver->solveBlocks(blocks, this->sparse_matrix);
118 ASSERT_TRUE(!state || blocks != nullptr);
119 if (!state) { // solver does not implement solving for a pattern return in this case
120 std::cerr << "Solver does not support solveBlocks()" << std::endl;
121 SUCCEED();
122 return;
123 }
124
125 for (size_t i = 0; i < this->sparse_matrix.rowBlockIndices().size(); ++i) {
126 int rr = this->sparse_matrix.rowBaseOfBlock(i);
127 int cc = this->sparse_matrix.colBaseOfBlock(i);
128 int numRows = this->sparse_matrix.rowsOfBlock(i);
129 int numCols = this->sparse_matrix.colsOfBlock(i);
130
131 g2o::MatrixX expected = this->matrix_inverse.block(rr, cc, numRows, numCols);
132 g2o::MatrixX::MapType actual(blocks[i], numRows, numCols);
133
134 EXPECT_TRUE(actual.isApprox(expected, 1e-6)) << "block " << i << " " << i << " differs";
135 }
136 TypeParam::first_type::deallocateBlocks(this->sparse_matrix, blocks);
137 }
138
139 // registering the test suite and all the types to be tested
140 REGISTER_TYPED_TEST_SUITE_P(LS, Solve, SolvePattern, SolveBlocks);
141
142 using LinearSolverTypes = ::testing::Types<
143 #ifdef G2O_HAVE_CSPARSE
144 std::pair<g2o::LinearSolverCSparse<g2o::MatrixX>, NoBlockOrdering>,
145 std::pair<g2o::LinearSolverCSparse<g2o::MatrixX>, BlockOrdering>,
146 #endif
147 #ifdef G2O_HAVE_CHOLMOD
148 std::pair<g2o::LinearSolverCholmod<g2o::MatrixX>, NoBlockOrdering>,
149 std::pair<g2o::LinearSolverCholmod<g2o::MatrixX>, BlockOrdering>,
150 #endif
151 std::pair<g2o::LinearSolverEigen<g2o::MatrixX>, NoBlockOrdering>,
152 std::pair<g2o::LinearSolverEigen<g2o::MatrixX>, BlockOrdering> >;
153 INSTANTIATE_TYPED_TEST_SUITE_P(LinearSolver, LS, LinearSolverTypes);
154