aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
blk-matrix.hpp
Go to the documentation of this file.
1
3#pragma once
4
5#include "aligator/math.hpp"
6#include <array>
7#include <numeric>
8
9namespace aligator {
10
17template <typename _MatrixType, int N, int M = N> class BlkMatrix;
18
19template <typename _MatrixType, int _N, int _M> class BlkMatrix {
20public:
21 using MatrixType = _MatrixType;
22 using PlainObject = typename MatrixType::PlainObject;
23 using Scalar = typename MatrixType::Scalar;
24 using Index = Eigen::Index;
25 enum { N = _N, M = _M, Options = PlainObject::Options };
26 static constexpr bool IsVectorAtCompileTime =
27 MatrixType::IsVectorAtCompileTime;
28
29 using RowDimsType = std::conditional_t<N != -1, std::array<Index, size_t(N)>,
30 std::vector<Index>>;
31 using ColDimsType = std::conditional_t<M != -1, std::array<Index, size_t(M)>,
32 std::vector<Index>>;
33
34 static_assert(N != 0 && M != 0,
35 "The BlkMatrix template class only supports nonzero numbers of "
36 "blocks in either direction.");
37
38 static_assert(!IsVectorAtCompileTime || (M == 1),
39 "Compile-time vector cannot have more than one column block.");
40
42 : m_data()
43 , m_rowDims()
44 , m_colDims()
45 , m_rowIndices()
46 , m_colIndices()
47 , m_totalRows(0)
48 , m_totalCols(0) {}
49
60
61 template <typename Other>
62 BlkMatrix(const Eigen::MatrixBase<Other> &data, const RowDimsType &rowDims,
63 const ColDimsType &colDims)
64 : m_data(data.derived())
69 , m_totalRows(0)
70 , m_totalCols(0) {
71 initialize();
72 }
73
74 template <typename Other>
75 BlkMatrix(Eigen::MatrixBase<Other> &data, const RowDimsType &rowDims,
76 const ColDimsType &colDims)
77 : m_data(data.derived())
82 , m_totalRows(0)
83 , m_totalCols(0) {
84 initialize();
85 }
86
88 template <typename Other>
89 BlkMatrix(const Eigen::MatrixBase<Other> &data, const RowDimsType &dims)
90 : BlkMatrix(data, dims, {data.cols()}) {}
91
93 template <typename Other>
94 BlkMatrix(Eigen::MatrixBase<Other> &data, const RowDimsType &dims)
95 : BlkMatrix(data, dims, {data.cols()}) {}
96
97 operator Eigen::Ref<PlainObject>() { return m_data; }
98 operator Eigen::Ref<const PlainObject>() const { return m_data; }
99
101 explicit BlkMatrix(const RowDimsType &dims)
102 : BlkMatrix(dims, {1}) {
103 static_assert(IsVectorAtCompileTime,
104 "Constructor only supported for vector types.");
105 }
106
108 inline auto operator()(size_t i, size_t j) {
109 return m_data.block(m_rowIndices[i], m_colIndices[j], m_rowDims[i],
110 m_colDims[j]);
111 }
112
114 inline auto operator()(size_t i, size_t j) const {
115 return m_data.block(m_rowIndices[i], m_colIndices[j], m_rowDims[i],
116 m_colDims[j]);
117 }
118
119 inline auto blockRow(size_t i) {
120 return m_data.middleRows(m_rowIndices[i], m_rowDims[i]);
121 }
122
123 inline auto blockRow(size_t i) const {
124 return m_data.middleRows(m_rowIndices[i], m_rowDims[i]);
125 }
126
127 inline auto blockCol(size_t j) const {
128 return m_data.middleCols(m_colIndices[j], m_colDims[j]);
129 }
130
131 inline auto blockCol(size_t j) {
132 return m_data.middleCols(m_colIndices[j], m_colDims[j]);
133 }
134
135 auto blockSegment(size_t i) {
136 EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType);
137 return m_data.segment(m_rowIndices[i], m_rowDims[i]);
138 }
139
140 auto blockSegment(size_t i) const {
141 EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType);
142 return m_data.segment(m_rowIndices[i], m_rowDims[i]);
143 }
144
145 inline auto operator[](size_t i) { return blockSegment(i); }
146
147 inline auto operator[](size_t i) const { return blockSegment(i); }
148
150 template <typename Other>
151 BlkMatrix &operator=(const Eigen::MatrixBase<Other> &other) {
152 assert(other.rows() == m_data.rows());
153 assert(other.cols() == m_data.cols());
154 m_data = other;
155 return *this;
156 }
157
158 void setZero() { m_data.setZero(); }
160 const ColDimsType &colDims) {
161
163 out.setZero();
164 return out;
165 }
166
167 template <typename Other> inline void swap(BlkMatrix<Other, N, M> &other) {
168 m_data.swap(other.matrix());
169 }
170
171 MatrixType &matrix() { return m_data; }
172 const MatrixType &matrix() const { return m_data; }
173
174 const RowDimsType &rowDims() const { return m_rowDims; }
175 const RowDimsType &rowIndices() const { return m_rowIndices; }
176 const ColDimsType &colDims() const { return m_colDims; }
177 const ColDimsType &colIndices() const { return m_colIndices; }
178
179 Index rows() const { return m_totalRows; }
180 Index cols() const { return m_totalCols; }
181
182 auto topBlkRows(size_t n) {
183 using OutType = BlkMatrix<Eigen::Ref<MatrixType>, -1, M>;
184 std::vector<Index> subRowDims;
185 subRowDims.resize(n);
186 std::copy_n(m_rowDims.cbegin(), n, subRowDims.begin());
187 Index ntr = std::accumulate(subRowDims.begin(), subRowDims.end(), 0);
188 return OutType(m_data.topRows(ntr), subRowDims, m_colDims);
189 }
190
191 template <size_t n> auto topBlkRows() {
192 static_assert(n <= N,
193 "Cannot take n block rows of matrix with <n block rows.");
194 using RefType = Eigen::Ref<MatrixType>;
195 using OutType = BlkMatrix<RefType, n, M>;
196 std::array<Index, n> subRowDims;
197 std::copy_n(m_rowDims.cbegin(), n, subRowDims.begin());
198 Index ntr = std::accumulate(subRowDims.begin(), subRowDims.end(), 0);
199 return OutType(m_data.topRows(ntr), subRowDims, m_colDims);
200 }
201
202 friend std::ostream &operator<<(std::ostream &oss, const BlkMatrix &self) {
203 return oss << self.m_data;
204 }
205
206protected:
214
215 void initialize() {
216 m_totalRows = 0;
217 m_totalCols = 0;
218 for (size_t i = 0; i < m_rowDims.size(); i++) {
221 }
222 for (size_t i = 0; i < m_colDims.size(); i++) {
225 }
227 }
228};
229
230} // namespace aligator
Block matrix class, with a fixed or dynamic-size number of row and column blocks.
auto blockSegment(size_t i)
void swap(BlkMatrix< Other, N, M > &other)
friend std::ostream & operator<<(std::ostream &oss, const BlkMatrix &self)
auto blockSegment(size_t i) const
Index cols() const
auto topBlkRows(size_t n)
typename MatrixType::PlainObject PlainObject
Index rows() const
std::conditional_t< N !=-1, std::array< Index, size_t(N)>, std::vector< Index > > RowDimsType
std::conditional_t< M !=-1, std::array< Index, size_t(M)>, std::vector< Index > > ColDimsType
const ColDimsType & colIndices() const
auto blockRow(size_t i)
MatrixType & matrix()
BlkMatrix(const Eigen::MatrixBase< Other > &data, const RowDimsType &dims)
Only-rows constructor (only for vectors)
static BlkMatrix Zero(const RowDimsType &rowDims, const ColDimsType &colDims)
auto blockCol(size_t j)
auto operator()(size_t i, size_t j)
Get the block in position ( i, j )
auto operator[](size_t i) const
auto blockCol(size_t j) const
auto operator()(size_t i, size_t j) const
const RowDimsType & rowIndices() const
const MatrixType & matrix() const
auto blockRow(size_t i) const
BlkMatrix(Eigen::MatrixBase< Other > &data, const RowDimsType &rowDims, const ColDimsType &colDims)
BlkMatrix(const RowDimsType &rowDims, const ColDimsType &colDims)
BlkMatrix(Eigen::MatrixBase< Other > &data, const RowDimsType &dims)
Only-rows constructor (only for vectors)
BlkMatrix & operator=(const Eigen::MatrixBase< Other > &other)
Set the data to be equal to some other Eigen object.
BlkMatrix(const Eigen::MatrixBase< Other > &data, const RowDimsType &rowDims, const ColDimsType &colDims)
BlkMatrix(const RowDimsType &dims)
Only-rows constructor (only for vectors)
auto operator[](size_t i)
Math utilities.
Main package namespace.