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 
8 
9 namespace drake {
10 
14 template <typename DerivedA, typename DerivedB>
15 bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
16  const Eigen::MatrixBase<DerivedB>& m2,
17  double tolerance) {
18  return (
19  (m1.rows() == m2.rows()) &&
20  (m1.cols() == m2.cols()) &&
21  ((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
22 }
23 
39 template <typename DerivedA, typename DerivedB>
41  const Eigen::MatrixBase<DerivedA>& m1,
42  const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
43  if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
44 
45  std::vector<bool> available(m2.cols());
46  for (int i = 0; i < m2.cols(); i++) available[i] = true;
47 
48  for (int i = 0; i < m1.cols(); i++) {
49  bool found_match = false;
50  for (int j = 0; j < m2.cols(); j++) {
51  if (available[j] &&
52  is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
53  found_match = true;
54  available[j] = false;
55  break;
56  }
57  }
58  if (!found_match) return false;
59  }
60  return true;
61 }
62 
63 
64 } // namespace drake
Definition: automotive_demo.cc:88
This header provides a std::make_unique implementation to be used when the compiler does not support ...
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:40
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:15