From ba3cc85701aaee6277ae5446e98027f9a1267295 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 06:17:14 -0500 Subject: [PATCH] avoid inheritance by conditionally defining backwards compatibility types/funcs in NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 566 ++++++++++++++++++------------ tests/testNonlinearFactor.cpp | 44 +++ 2 files changed, 385 insertions(+), 225 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 34e891c64..764f1fdf9 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -306,6 +306,76 @@ public: }; // \class NoiseModelFactor +/* ************************************************************************* */ +/* We need some helper structs to help us with NoiseModelFactorN - specifically + * we need to alias X1, X2, X3, ... in the templated NoiseModelFactorN class to + * maintain backwards compatibility with NoiseModelFactor1, NoiseModelFactor2, + * NoiseModelFactor3, ... + * + * The tricky part is that we want to _conditionally_ alias these only if the + * `sizeof...(VALUES)` is greater than the index we want to alias (e.g. a 3-way + * factor should only have up to X3). + * + * The approach we use is to create structs which use template specialization to + * conditionally typedef X1, X2, ... for us, then inherit from them to inherit + * the aliases. + * + * Usage: + * ``` + * template + * class MyClass : public AliasX3 { ... }; + * ``` + * This will only typedef X3 if VALUES has at least 3 template parameters. So + * then we can do something like: + * ``` + * int main { + * MyClass::X3 a; // variable a will have type double + * // MyClass::X3 b; // this won't compile + * MyClass::X3 c; // variable c will have type char + * } + * ``` + */ + +// convenience macro extracts the type for the i'th VALUE in a parameter pack +#define GET_VALUE_I(VALUES, I) \ + typename std::tuple_element>::type + +namespace detail { + +// First handle `typedef X`. By default, we do not alias X (empty struct). +template +struct AliasX_ {}; +// But if `1 == sizeof...(VALUES)` is true, then we do alias X by specializing +// for when the first template parameter is true. +template +struct AliasX_<(1 == sizeof...(VALUES)), VALUES...> { + using X = GET_VALUE_I(VALUES, 0); +}; +// We'll alias the "true" template version for convenience. +template +using AliasX = AliasX_; + +// Now do the same thing for X1, X2, ... using a macro. +#define ALIAS_HELPER_X(N) \ + template \ + struct AliasX##N##_ {}; \ + template \ + struct AliasX##N##_<(N <= sizeof...(VALUES)), VALUES...> { \ + using X##N = GET_VALUE_I(VALUES, N - 1); \ + }; \ + template \ + using AliasX##N = AliasX##N##_; +ALIAS_HELPER_X(1); +ALIAS_HELPER_X(2); +ALIAS_HELPER_X(3); +ALIAS_HELPER_X(4); +ALIAS_HELPER_X(5); +ALIAS_HELPER_X(6); +#undef ALIAS_HELPER_X +#undef GET_VALUE_I + +} // namespace detail + /* ************************************************************************* */ /** * A convenient base class for creating your own NoiseModelFactor @@ -337,11 +407,21 @@ public: * objects in non-linear manifolds (Lie groups). */ template -class NoiseModelFactorN : public NoiseModelFactor { +class NoiseModelFactorN : public NoiseModelFactor, + public detail::AliasX, + public detail::AliasX1, + public detail::AliasX2, + public detail::AliasX3, + public detail::AliasX4, + public detail::AliasX5, + public detail::AliasX6 { public: - /** The type of the N'th template param can be obtained as VALUE */ - template - using VALUE = typename std::tuple_element>::type; + /// N is the number of variables (N-way factor) + enum { N = sizeof...(VALUES) }; + + /** The type of the i'th template param can be obtained as VALUE */ + template ::type = true> + using VALUE = typename std::tuple_element>::type; protected: using Base = NoiseModelFactor; @@ -375,28 +455,42 @@ class NoiseModelFactorN : public NoiseModelFactor { */ NoiseModelFactorN(const SharedNoiseModel& noiseModel, key_type... keys) - : Base(noiseModel, std::array{keys...}) {} + : Base(noiseModel, std::array{keys...}) {} /** - * Constructor. + * Constructor. Only enabled for n-ary factors where n > 1. * @param noiseModel Shared pointer to noise model. * @param keys A container of keys for the variables in this factor. */ - template + template 1), bool>::type = true> NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { - assert(keys.size() == sizeof...(VALUES)); + assert(keys.size() == N); } /// @} ~NoiseModelFactorN() override {} - /** Method to retrieve keys */ - template - inline Key key() const { - return keys_[N]; + /** Methods to retrieve keys */ +#define SUB(Old, New) template // to delay template deduction +#define KEY_IF_TRUE(Enable) typename std::enable_if<(Enable), Key>::type + // templated version of `key()` + template + inline KEY_IF_TRUE(I < N) key() const { + return keys_[I]; } + // backwards-compatibility functions + SUB(T, N) inline KEY_IF_TRUE(T == 1) key() const { return keys_[0]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 1) key1() const { return keys_[0]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 2) key2() const { return keys_[1]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 3) key3() const { return keys_[2]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 4) key4() const { return keys_[3]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 5) key5() const { return keys_[4]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 6) key6() const { return keys_[5]; } +#undef SUB +#undef KEY_IF_TRUE /// @name NoiseModelFactor methods /// @{ @@ -458,8 +552,7 @@ class NoiseModelFactorN : public NoiseModelFactor { /** Some optional jacobians omitted function overload */ template 0) && - (sizeof...(OptionalJacArgs) < - sizeof...(VALUES)), + (sizeof...(OptionalJacArgs) < N), bool>::type = true> inline Vector evaluateError(const VALUES&... x, OptionalJacArgs&&... H) const { @@ -498,245 +591,268 @@ class NoiseModelFactorN : public NoiseModelFactor { } }; // \class NoiseModelFactorN -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 1 variable. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor1 : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X = VALUE; +// template +// using NoiseModelFactor1 = NoiseModelFactorN; +// template +// using NoiseModelFactor2 = NoiseModelFactorN; +// template +// using NoiseModelFactor3 = NoiseModelFactorN; +// template +// using NoiseModelFactor4 = NoiseModelFactorN; +// template +// using NoiseModelFactor5 = +// NoiseModelFactorN; +// template +// using NoiseModelFactor6 = +// NoiseModelFactorN; - protected: - using Base = NoiseModelFactor; // grandparent, for backwards compatibility - using This = NoiseModelFactor1; +#define NoiseModelFactor1 NoiseModelFactorN +#define NoiseModelFactor2 NoiseModelFactorN +#define NoiseModelFactor3 NoiseModelFactorN +#define NoiseModelFactor4 NoiseModelFactorN +#define NoiseModelFactor5 NoiseModelFactorN +#define NoiseModelFactor6 NoiseModelFactorN - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor1() override {} +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 1 variable. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor1 : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X = VALUE; - /** method to retrieve key */ - inline Key key() const { return this->keys_[0]; } +// protected: +// using Base = NoiseModelFactor; // grandparent, for backwards compatibility +// using This = NoiseModelFactor1; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor1 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor1() override {} -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 2 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor2 : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; +// /** method to retrieve key */ +// inline Key key() const { return this->keys_[0]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor2; +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor1 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor2() override {} +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 2 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor2 : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; - /** methods to retrieve both keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor2; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor2 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor2() override {} -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 3 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor3 : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; +// /** methods to retrieve both keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor3; +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor2 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor3() override {} +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 3 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor3 : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor3; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor3 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor3() override {} -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 4 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor4 - : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor4; +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor3 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor4() override {} +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 4 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor4 +// : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; +// using X4 = VALUE4; - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor4; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor4 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor4() override {} -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 5 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor5 - : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; - using X5 = VALUE5; +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } +// inline Key key4() const { return this->keys_[3]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor5; +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor4 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor5() override {} +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 5 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor5 +// : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; +// using X4 = VALUE4; +// using X5 = VALUE5; - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } - inline Key key5() const { return this->keys_[4]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor5; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor5 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor5() override {} -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 6 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor6 - : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; - using X5 = VALUE5; - using X6 = VALUE6; +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } +// inline Key key4() const { return this->keys_[3]; } +// inline Key key5() const { return this->keys_[4]; } - protected: - using Base = NoiseModelFactor; - using This = - NoiseModelFactor6; +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor5 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor6() override {} +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 6 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor6 +// : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; +// using X4 = VALUE4; +// using X5 = VALUE5; +// using X6 = VALUE6; - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } - inline Key key5() const { return this->keys_[4]; } - inline Key key6() const { return this->keys_[5]; } +// protected: +// using Base = NoiseModelFactor; +// using This = +// NoiseModelFactor6; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor6 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor6() override {} + +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } +// inline Key key4() const { return this->keys_[3]; } +// inline Key key5() const { return this->keys_[4]; } +// inline Key key6() const { return this->keys_[5]; } + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor6 } // \namespace gtsam diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index fda65d56a..fba7949a1 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -253,6 +253,50 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +class TestFactor1 : public NoiseModelFactor1 { + static_assert(std::is_same::value, "Base type wrong"); + static_assert(std::is_same>::value, + "This type wrong"); + + public: + typedef NoiseModelFactor1 Base; + TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} + + Vector evaluateError(const double& x1, boost::optional H1 = + boost::none) const override { + if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + return (Vector(1) << x1).finished(); + } + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); + } +}; + +/* ************************************ */ +TEST(NonlinearFactor, NoiseModelFactor1) { + TestFactor1 tf; + Values tv; + tv.insert(L(1), double((1.0))); + EXPECT(assert_equal((Vector(1) << 1.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.25 / 2.0, tf.error(tv), 1e-9); + JacobianFactor jf( + *boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)L(1), (long)jf.keys()[0]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), + jf.getA(jf.begin()))); + EXPECT(assert_equal((Vector)(Vector(1) << -0.5).finished(), jf.getb())); + + // Test all functions/types for backwards compatibility + static_assert(std::is_same::value, + "X type incorrect"); + EXPECT(assert_equal(tf.key(), L(1))); + std::vector H = {Matrix()}; + EXPECT(assert_equal(Vector1(1.0), tf.unwhitenedError(tv, H))); +} + /* ************************************************************************* */ class TestFactor4 : public NoiseModelFactor4 { static_assert(std::is_same::value, "Base type wrong");