diff --git a/base/Makefile.am b/base/Makefile.am index 6b7aa2e7d..ef0b1930b 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -19,7 +19,7 @@ check_PROGRAMS += tests/testSPQRUtil endif # Testing -headers += Testable.h numericalDerivative.h +headers += Testable.h TestableAssertions.h numericalDerivative.h # Lie Groups headers += Lie.h Lie-inl.h diff --git a/base/TestableAssertions.h b/base/TestableAssertions.h new file mode 100644 index 000000000..59ab43da3 --- /dev/null +++ b/base/TestableAssertions.h @@ -0,0 +1,34 @@ +/** + * @file TestableAssertions.h + * @brief Provides additional testing facilities for common data structures + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include "Testable.h" + +namespace gtsam { + +/** + * Version of assert_equals to work with vectors + */ +template +bool assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { + if (expected.size() != actual.size()) { + printf("Sizes not equal:\n"); + printf("expected size: %lu\n", expected.size()); + printf("actual size: %lu\n", actual.size()); + return false; + } + size_t i = 0; + BOOST_FOREACH(const V& a, expected) { + if (!assert_equal(a, expected[i++], tol)) + return false; + } + return true; +} + +} // \namespace gtsam