forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
matrix_util.h
155 lines (139 loc) · 5.96 KB
/
matrix_util.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#pragma once
#include <algorithm>
#include <cmath>
#include <functional>
#include <Eigen/Dense>
#include "drake/common/drake_assert.h"
#include "drake/common/drake_throw.h"
#include "drake/common/eigen_types.h"
namespace drake {
namespace math {
/// Determines if a matrix is symmetric. If std::equal_to<>()(matrix(i, j),
/// matrix(j, i)) is true for all i, j, then the matrix is symmetric.
template <typename Derived>
bool IsSymmetric(const Eigen::MatrixBase<Derived>& matrix) {
using DerivedScalar = typename Derived::Scalar;
if (matrix.rows() != matrix.cols()) {
return false;
}
for (int i = 0; i < static_cast<int>(matrix.rows()); ++i) {
for (int j = i + 1; j < static_cast<int>(matrix.cols()); ++j) {
if (!std::equal_to<DerivedScalar>()(matrix(i, j), matrix(j, i))) {
return false;
}
}
}
return true;
}
/// Determines if a matrix is symmetric based on whether the difference between
/// matrix(i, j) and matrix(j, i) is smaller than @p precision for all i, j.
/// The precision is absolute.
/// Matrix with nan or inf entries is not allowed.
template <typename Derived>
bool IsSymmetric(const Eigen::MatrixBase<Derived>& matrix,
const typename Derived::Scalar& precision) {
if (!std::isfinite(precision)) {
throw std::runtime_error("Cannot accept nans or inf is IsSymmetric");
}
using DerivedScalar = typename Derived::Scalar;
if (matrix.rows() != matrix.cols()) {
return false;
}
for (int i = 0; i < static_cast<int>(matrix.rows()); ++i) {
if (!std::isfinite(matrix(i, i))) {
throw std::runtime_error("Cannot accept nans or inf is IsSymmetric");
}
for (int j = i + 1; j < static_cast<int>(matrix.rows()); ++j) {
if (!std::isfinite(matrix(i, j)) || !std::isfinite(matrix(j, i))) {
throw std::runtime_error("Cannot accept nans or inf is IsSymmetric");
}
DerivedScalar diff = matrix(i, j) - matrix(j, i);
if (!Eigen::NumTraits<DerivedScalar>::IsSigned) {
if (diff > precision) {
return false;
}
} else if (diff > precision || -diff > precision) {
return false;
}
}
}
return true;
}
namespace internal {
template <typename Derived1, typename Derived2>
void to_symmetric_matrix_from_lower_triangular_columns_impl(
int rows, const Eigen::MatrixBase<Derived1>& lower_triangular_columns,
Eigen::MatrixBase<Derived2>* symmetric_matrix) {
int count = 0;
for (int j = 0; j < rows; ++j) {
(*symmetric_matrix)(j, j) = lower_triangular_columns(count);
++count;
for (int i = j + 1; i < rows; ++i) {
(*symmetric_matrix)(i, j) = lower_triangular_columns(count);
(*symmetric_matrix)(j, i) = lower_triangular_columns(count);
++count;
}
}
}
} // namespace internal
/// Given a column vector containing the stacked columns of the lower triangular
/// part of a square matrix, returning a symmetric matrix whose lower
/// triangular part is the same as the original matrix.
template <typename Derived>
drake::MatrixX<typename Derived::Scalar>
ToSymmetricMatrixFromLowerTriangularColumns(
const Eigen::MatrixBase<Derived>& lower_triangular_columns) {
int rows = (-1 + sqrt(1 + 8 * lower_triangular_columns.rows())) / 2;
DRAKE_ASSERT(rows * (rows + 1) / 2 == lower_triangular_columns.rows());
DRAKE_ASSERT(lower_triangular_columns.cols() == 1);
drake::MatrixX<typename Derived::Scalar> symmetric_matrix(rows, rows);
internal::to_symmetric_matrix_from_lower_triangular_columns_impl(
rows, lower_triangular_columns, &symmetric_matrix);
return symmetric_matrix;
}
/// Given a column vector containing the stacked columns of the lower triangular
/// part of a square matrix, returning a symmetric matrix whose lower
/// triangular part is the same as the original matrix.
/// @tparam rows The number of rows in the symmetric matrix.
template <int rows, typename Derived>
Eigen::Matrix<typename Derived::Scalar, rows, rows>
ToSymmetricMatrixFromLowerTriangularColumns(
const Eigen::MatrixBase<Derived>& lower_triangular_columns) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, rows * (rows + 1) / 2);
Eigen::Matrix<typename Derived::Scalar, rows, rows> symmetric_matrix(rows,
rows);
internal::to_symmetric_matrix_from_lower_triangular_columns_impl(
rows, lower_triangular_columns, &symmetric_matrix);
return symmetric_matrix;
}
/// Checks if a matrix is symmetric (with tolerance @p symmetry_tolerance --
/// @see IsSymmetric) and has all eigenvalues greater than @p
/// eigenvalue_tolerance. @p eigenvalue_tolerance must be >= 0 -- where 0
/// implies positive semi-definite (but is of course subject to all of the
/// pitfalls of floating point).
///
/// To consider the numerical robustness of the eigenvalue estimation, we
/// specifically check that min_eigenvalue >= eigenvalue_tolerance * max(1,
/// max_abs_eigenvalue).
template <typename Derived>
bool IsPositiveDefinite(const Eigen::MatrixBase<Derived>& matrix,
double eigenvalue_tolerance = 0.0,
double symmetry_tolerance = 0.0) {
DRAKE_DEMAND(eigenvalue_tolerance >= 0);
DRAKE_DEMAND(symmetry_tolerance >= 0);
if (!IsSymmetric(matrix, symmetry_tolerance)) return false;
// Note: Eigen's documentation clearly warns against using the faster LDLT
// for this purpose, as the algorithm cannot handle indefinite matrices.
Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> eigensolver(
matrix);
DRAKE_THROW_UNLESS(eigensolver.info() == Eigen::Success);
// According to the Lapack manual, the absolute accuracy of eigenvalues is
// eps*max(|eigenvalues|), so I will write my tolerances relative to that.
// Anderson et al., Lapack User's Guide, 3rd ed. section 4.7, 1999.
const double max_abs_eigenvalue =
eigensolver.eigenvalues().cwiseAbs().maxCoeff();
return eigensolver.eigenvalues().minCoeff() >=
eigenvalue_tolerance * std::max(1., max_abs_eigenvalue);
}
} // namespace math
} // namespace drake