Drake
is_approx_equal_abstol.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <vector>
4 
5 #include <Eigen/Dense>
6 
7 namespace drake {
8 
9 /// Returns true if and only if the two matrices are equal to within a certain
10 /// absolute elementwise @p tolerance. Special values (infinities, NaN, etc.)
11 /// do not compare as equal elements.
12 template <typename DerivedA, typename DerivedB>
13 bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
14  const Eigen::MatrixBase<DerivedB>& m2,
15  double tolerance) {
16  return (
17  (m1.rows() == m2.rows()) &&
18  (m1.cols() == m2.cols()) &&
19  ((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
20 }
21 
22 /// Returns true if and only if a simple greedy search reveals a permutation
23 /// of the columns of m2 to make the matrix equal to m1 to within a certain
24 /// absolute elementwise @p tolerance. E.g., there exists a P such that
25 /// <pre>
26 /// forall i,j, |m1 - m2*P|_{i,j} <= tolerance
27 /// where P is a permutation matrix:
28 /// P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
29 /// </pre>
30 /// Note: Returns false for matrices of different sizes.
31 /// Note: The current implementation is O(n^2) in the number of columns.
32 /// Note: In marginal cases (with similar but not identical columns) this
33 /// algorithm can fail to find a permutation P even if it exists because it
34 /// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the
35 /// pool. It is possible that other columns of m2 would also match m1(i) but
36 /// that m2(j) is the only match possible for a later column of m1.
37 template <typename DerivedA, typename DerivedB>
39  const Eigen::MatrixBase<DerivedA>& m1,
40  const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
41  if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
42 
43  std::vector<bool> available(m2.cols());
44  for (int i = 0; i < m2.cols(); i++) available[i] = true;
45 
46  for (int i = 0; i < m1.cols(); i++) {
47  bool found_match = false;
48  for (int j = 0; j < m2.cols(); j++) {
49  if (available[j] &&
50  is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
51  found_match = true;
52  available[j] = false;
53  break;
54  }
55  }
56  if (!found_match) return false;
57  }
58  return true;
59 }
60 
61 
62 } // namespace drake
Definition: automotive_demo.cc:88
bool IsApproxEqualAbsTolWithPermutedColumns(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance)
Returns true if and only if a simple greedy search reveals a permutation of the columns of m2 to make...
Definition: is_approx_equal_abstol.h:38
bool is_approx_equal_abstol(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance)
Returns true if and only if the two matrices are equal to within a certain absolute elementwise toler...
Definition: is_approx_equal_abstol.h:13