aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
block-tridiagonal.hpp
Go to the documentation of this file.
1
3#pragma once
4
7#include "aligator/tracy.hpp"
8
9namespace aligator {
10namespace gar {
11
12template <typename MatrixType, class A>
13typename MatrixType::PlainObject
14blockTridiagToDenseMatrix(const std::vector<MatrixType, A> &subdiagonal,
15 const std::vector<MatrixType, A> &diagonal,
16 const std::vector<MatrixType, A> &superdiagonal) {
17 if (subdiagonal.size() != superdiagonal.size() ||
18 diagonal.size() != superdiagonal.size() + 1) {
19 ALIGATOR_DOMAIN_ERROR("Wrong lengths");
20 }
21
22 using PlainObjectType = typename MatrixType::PlainObject;
23
24 const size_t N = subdiagonal.size();
25 Eigen::Index dim = 0;
26 for (size_t i = 0; i <= N; i++) {
27 dim += diagonal[i].cols();
28 }
29
30 PlainObjectType out(dim, dim);
31 out.setZero();
32 Eigen::Index i0 = 0;
33 for (size_t i = 0; i <= N; i++) {
34 Eigen::Index d = diagonal[i].cols();
35 out.block(i0, i0, d, d) = diagonal[i];
36
37 if (i != N) {
38 Eigen::Index dn = superdiagonal[i].cols();
39 out.block(i0, i0 + d, d, dn) = superdiagonal[i];
40 out.block(i0 + d, i0, dn, d) = subdiagonal[i];
41 }
42
43 i0 += d;
44 }
45 return out;
46}
47
50template <typename MatrixType, class A, typename InputType, typename OutType,
51 typename Scalar = typename MatrixType::Scalar>
52bool blockTridiagMatMul(const std::vector<MatrixType, A> &Asub,
53 const std::vector<MatrixType, A> &Adiag,
54 const std::vector<MatrixType, A> &Asuper,
56 BlkMatrix<OutType, -1, 1> &c, const Scalar beta) {
58 const size_t N = Asuper.size();
59
60 c.matrix() *= beta;
61
62 c[0].noalias() += Adiag[0] * b[0];
63 c[0].noalias() += Asuper[0] * b[1];
64
65 for (size_t i = 1; i < N; i++) {
66 c[i].noalias() += Asub[i - 1] * b[i - 1];
67 c[i].noalias() += Adiag[i] * b[i];
68 c[i].noalias() += Asuper[i] * b[i + 1];
69 }
70
71 c[N].noalias() += Asub[N - 1] * b[N - 1];
72 c[N].noalias() += Adiag[N] * b[N];
73
74 return true;
75}
76
80template <typename MatrixType, class A, class A2, typename RhsType,
81 typename DecType>
82bool symmetricBlockTridiagSolve(std::vector<MatrixType, A> &subdiagonal,
83 std::vector<MatrixType, A> &diagonal,
84 const std::vector<MatrixType, A> &superdiagonal,
86 std::vector<DecType, A2> &facs) {
87 ALIGATOR_TRACY_ZONE_SCOPED;
89
90 if (subdiagonal.size() != superdiagonal.size() ||
91 diagonal.size() != superdiagonal.size() + 1 ||
92 rhs.rowDims().size() != diagonal.size()) {
93 return false;
94 }
95
96 // size of problem
97 const size_t N = superdiagonal.size();
98
99 size_t i = N - 1;
100 while (true) {
101 DecType &ldl = facs[i + 1];
102 ldl.compute(diagonal[i + 1]);
103 if (ldl.info() != Eigen::Success)
104 return false;
105
106 Eigen::Ref<RhsType> r = rhs[i + 1];
107 ldl.solveInPlace(r);
108
109 // the math has index of B starting at 1, array starts at 0
110 const auto &Bip1 = superdiagonal[i];
111 auto &Cip1 = subdiagonal[i]; // should be Bi.transpose()
112
113 rhs[i].noalias() -= Bip1 * rhs[i + 1];
114 ldl.solveInPlace(Cip1); // contains U.T = D[i+1]^-1 * B[i+1].transpose()
115
116 diagonal[i].noalias() -= Bip1 * Cip1;
117
118 if (i == 0)
119 break;
120 i--;
121 }
122
123 {
124 DecType &ldl = facs[0];
125 ldl.compute(diagonal[0]);
126 if (ldl.info() != Eigen::Success)
127 return false;
128 Eigen::Ref<RhsType> r = rhs[0];
129 ldl.solveInPlace(r);
130 }
131
132 for (size_t i = 0; i < N; i++) {
133 const auto &Uip1t = subdiagonal[i];
134 rhs[i + 1].noalias() -= Uip1t * rhs[i];
135 }
136
137 return true;
138}
139
145template <typename MatrixType, class A, class A2, typename RhsType,
146 typename DecType>
148 const std::vector<MatrixType, A> &transposedUfacs,
149 const std::vector<MatrixType, A> &superdiagonal,
150 const std::vector<DecType, A2> &diagonalFacs,
153 // size of problem
154 const size_t N = superdiagonal.size();
155
156 size_t i = N - 1;
157 while (true) {
158 const DecType &ldl = diagonalFacs[i + 1];
159 Eigen::Ref<RhsType> r = rhs[i + 1];
160 ldl.solveInPlace(r);
161
162 // the math has index of B starting at 1, array starts at 0
163 const auto &Bip1 = superdiagonal[i];
164
165 rhs[i].noalias() -= Bip1 * rhs[i + 1];
166
167 if (i == 0)
168 break;
169 i--;
170 }
171
172 const DecType &ldl = diagonalFacs[0];
173 Eigen::Ref<RhsType> r = rhs[0];
174 ldl.solveInPlace(r);
175
176 for (size_t i = 0; i < N; i++) {
177 const auto &Uip1t = transposedUfacs[i];
178 rhs[i + 1].noalias() -= Uip1t * rhs[i];
179 }
180
181 return true;
182}
183
186template <typename MatrixType, class A, class A2, typename RhsType,
187 typename DecType>
189 std::vector<MatrixType, A> &subdiagonal,
190 std::vector<MatrixType, A> &diagonal,
191 std::vector<MatrixType, A> &superdiagonal, BlkMatrix<RhsType, -1, 1> &rhs,
192 std::vector<DecType, A2> &facs) {
193 ALIGATOR_TRACY_ZONE_SCOPED;
195
196 if (subdiagonal.size() != superdiagonal.size() ||
197 diagonal.size() != superdiagonal.size() + 1 ||
198 rhs.rowDims().size() != diagonal.size()) {
199 return false;
200 }
201
202 // size of problem
203 const size_t N = superdiagonal.size();
204
205 for (size_t i = 0; i < N; i++) {
206 DecType &ldl = facs[i];
207 ldl.compute(diagonal[i]);
208 if (ldl.info() != Eigen::Success)
209 return false;
210
211 Eigen::Ref<RhsType> r = rhs[i];
212 ldl.solveInPlace(r);
213
214 // the math has index of B starting at 1, array starts at 0
215 auto &Bip1 = superdiagonal[i];
216 const auto &Cip1 = subdiagonal[i]; // should be B[i+1].transpose()
217
218 rhs[i + 1].noalias() -= Cip1 * rhs[i];
219 ldl.solveInPlace(Bip1); // contains L.T = D[i]^-1 * B[i+1]
220
221 // substract B[i+1].T * L.T
222 diagonal[i + 1].noalias() -= Cip1 * Bip1;
223 }
224
225 {
226 DecType &ldl = facs[N];
227 ldl.compute(diagonal[N]);
228 if (ldl.info() != Eigen::Success)
229 return false;
230 Eigen::Ref<RhsType> r = rhs[N];
231 ldl.solveInPlace(r);
232 }
233
234 size_t i = N - 1;
235 while (true) {
236 const auto &Lim1t = superdiagonal[i];
237 rhs[i].noalias() -= Lim1t * rhs[i + 1];
238
239 if (i == 0)
240 break;
241 i--;
242 }
243 return true;
244}
245
246} // namespace gar
247} // namespace aligator
Block matrix class, with a fixed or dynamic-size number of row and column blocks.
MatrixType & matrix()
const RowDimsType & rowDims() const
#define ALIGATOR_DOMAIN_ERROR(...)
#define ALIGATOR_NOMALLOC_SCOPED
Definition math.hpp:44
bool symmetricBlockTridiagSolve(std::vector< MatrixType, A > &subdiagonal, std::vector< MatrixType, A > &diagonal, const std::vector< MatrixType, A > &superdiagonal, BlkMatrix< RhsType, -1, 1 > &rhs, std::vector< DecType, A2 > &facs)
Solve a symmetric block-tridiagonal problem by in-place factorization. The subdiagonal will be used ...
bool symmetricBlockTridiagSolveDownLooking(std::vector< MatrixType, A > &subdiagonal, std::vector< MatrixType, A > &diagonal, std::vector< MatrixType, A > &superdiagonal, BlkMatrix< RhsType, -1, 1 > &rhs, std::vector< DecType, A2 > &facs)
Solve a symmetric block-tridiagonal problem by in-place factorization. The subdiagonal will be used ...
bool blockTridiagMatMul(const std::vector< MatrixType, A > &Asub, const std::vector< MatrixType, A > &Adiag, const std::vector< MatrixType, A > &Asuper, const BlkMatrix< InputType, -1, 1 > &b, BlkMatrix< OutType, -1, 1 > &c, const Scalar beta)
Evaluate c <- beta * c + A * b, where A is the block-tridiagonal matrix described by the first three ...
MatrixType::PlainObject blockTridiagToDenseMatrix(const std::vector< MatrixType, A > &subdiagonal, const std::vector< MatrixType, A > &diagonal, const std::vector< MatrixType, A > &superdiagonal)
bool blockTridiagRefinementStep(const std::vector< MatrixType, A > &transposedUfacs, const std::vector< MatrixType, A > &superdiagonal, const std::vector< DecType, A2 > &diagonalFacs, BlkMatrix< RhsType, -1, 1 > &rhs)
Given the decomposed matrix data, just perform the backward-forward step of the algorithm.
Main package namespace.