diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4bb8da685..6d6162825 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -19,9 +19,11 @@ #pragma once +#include +#include +#include #include #include -#include #include namespace gtsam { @@ -34,6 +36,9 @@ namespace gtsam { * such instances, the user should declare derived classes from this template, * implementing expresion(), serialize(), clone(), print(), and defining the * corresponding `struct traits : public Testable {}`. + * + * \tparam T Type for measurements. + * */ template class ExpressionFactor: public NoiseModelFactor { @@ -222,26 +227,98 @@ private: template struct traits > : public Testable > {}; +/** + * N-ary variadic template for ExpressionFactor meant as a base class for N-ary + * factors. Enforces an 'expression' method with N keys. + * Derived class (an N-factor!) needs to call 'initialize'. + * + * Does not provide backward compatible 'evaluateError'. + * + * \tparam T Type for measurements. The rest of template arguments are types + * for the N key-indexed Values. + * + */ +template +class ExpressionFactorN : public ExpressionFactor { +public: + static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); + using ArrayNKeys = std::array; + + /// Destructor + virtual ~ExpressionFactorN() = default; + + // Don't provide backward compatible evaluateVector(), due to its problematic + // variable length of optional Jacobian arguments. Vector evaluateError(const + // Args... args,...); + + /// Recreate expression from given keys_ and measured_, used in load + /// Needed to deserialize a derived factor + virtual Expression expression(const ArrayNKeys &keys) const { + throw std::runtime_error( + "ExpressionFactorN::expression not provided: cannot deserialize."); + } + +protected: + /// Default constructor, for serialization + ExpressionFactorN() = default; + + /// Constructor takes care of keys, but still need to call initialize + ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel, + const T &measurement) + : ExpressionFactor(noiseModel, measurement) { + for (const auto &key : keys) + Factor::keys_.push_back(key); + } + +private: + /// Return an expression that predicts the measurement given Values + Expression expression() const override { + ArrayNKeys keys; + int idx = 0; + for (const auto &key : Factor::keys_) + keys[idx++] = key; + return expression(keys); + } + + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &boost::serialization::make_nvp( + "ExpressionFactorN", + boost::serialization::base_object>(*this)); + } +}; +/// traits +template +struct traits> + : public Testable> {}; +// ExpressionFactorN + + +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V41) /** * Binary specialization of ExpressionFactor meant as a base class for binary - * factors. Enforces an 'expression' method with two keys, and provides 'evaluateError'. - * Derived class (a binary factor!) needs to call 'initialize'. + * factors. Enforces an 'expression' method with two keys, and provides + * 'evaluateError'. Derived class (a binary factor!) needs to call 'initialize'. + * + * \sa ExpressionFactorN + * \deprecated Prefer the more general ExpressionFactorN<>. */ template -class ExpressionFactor2 : public ExpressionFactor { - public: +class ExpressionFactor2 : public ExpressionFactorN { +public: /// Destructor - virtual ~ExpressionFactor2() {} + ~ExpressionFactor2() override {} /// Backwards compatible evaluateError, to make existing tests compile - Vector evaluateError(const A1& a1, const A2& a2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + Vector evaluateError(const A1 &a1, const A2 &a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { Values values; values.insert(this->keys_[0], a1); values.insert(this->keys_[1], a2); std::vector H(2); - Vector error = this->unwhitenedError(values, H); + Vector error = ExpressionFactor::unwhitenedError(values, H); if (H1) (*H1) = H[0]; if (H2) (*H2) = H[1]; return error; @@ -250,35 +327,25 @@ class ExpressionFactor2 : public ExpressionFactor { /// Recreate expression from given keys_ and measured_, used in load /// Needed to deserialize a derived factor virtual Expression expression(Key key1, Key key2) const { - throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize."); + throw std::runtime_error( + "ExpressionFactor2::expression not provided: cannot deserialize."); + } + virtual Expression + expression(const typename ExpressionFactorN::ArrayNKeys &keys) + const override { + return expression(keys[0], keys[1]); } - protected: +protected: /// Default constructor, for serialization ExpressionFactor2() {} /// Constructor takes care of keys, but still need to call initialize - ExpressionFactor2(Key key1, Key key2, - const SharedNoiseModel& noiseModel, - const T& measurement) - : ExpressionFactor(noiseModel, measurement) { - this->keys_.push_back(key1); - this->keys_.push_back(key2); - } - - private: - /// Return an expression that predicts the measurement given Values - Expression expression() const override { - return expression(this->keys_[0], this->keys_[1]); - } - - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "ExpressionFactor", boost::serialization::base_object >(*this)); - } + ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel, + const T &measurement) + : ExpressionFactorN({key1, key2}, noiseModel, measurement) {} }; // ExpressionFactor2 +#endif -}// \ namespace gtsam +} // namespace gtsam diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index 530fcfdcd..7076708db 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -34,8 +34,8 @@ struct Bearing; */ template ::result_type> -struct BearingFactor : public ExpressionFactor2 { - typedef ExpressionFactor2 Base; +struct BearingFactor : public ExpressionFactorN { + typedef ExpressionFactorN Base; /// default constructor BearingFactor() {} @@ -43,14 +43,14 @@ struct BearingFactor : public ExpressionFactor2 { /// primary constructor BearingFactor(Key key1, Key key2, const T& measured, const SharedNoiseModel& model) - : Base(key1, key2, model, measured) { - this->initialize(expression(key1, key2)); + : Base({key1, key2}, model, measured) { + this->initialize(expression({key1, key2})); } // Return measurement expression - Expression expression(Key key1, Key key2) const override { - Expression a1_(key1); - Expression a2_(key2); + Expression expression(const typename Base::ArrayNKeys &keys) const override { + Expression a1_(keys[0]); + Expression a2_(keys[1]); return Expression(Bearing(), a1_, a2_); } @@ -60,6 +60,21 @@ struct BearingFactor : public ExpressionFactor2 { std::cout << s << "BearingFactor" << std::endl; Base::print(s, kf); } + + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + std::vector Hs(2); + const auto &keys = Factor::keys(); + const Vector error = unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, + Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; + } + private: friend class boost::serialization::access; diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index be645ef94..af7b47446 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -33,10 +33,10 @@ template ::result_type, typename R = typename Range::result_type> class BearingRangeFactor - : public ExpressionFactor2, A1, A2> { + : public ExpressionFactorN, A1, A2> { private: typedef BearingRange T; - typedef ExpressionFactor2 Base; + typedef ExpressionFactorN Base; typedef BearingRangeFactor This; public: @@ -48,8 +48,8 @@ class BearingRangeFactor /// primary constructor BearingRangeFactor(Key key1, Key key2, const B& measuredBearing, const R& measuredRange, const SharedNoiseModel& model) - : Base(key1, key2, model, T(measuredBearing, measuredRange)) { - this->initialize(expression(key1, key2)); + : Base({key1, key2}, model, T(measuredBearing, measuredRange)) { + this->initialize(expression({key1, key2})); } virtual ~BearingRangeFactor() {} @@ -61,9 +61,23 @@ class BearingRangeFactor } // Return measurement expression - Expression expression(Key key1, Key key2) const override { - return Expression(T::Measure, Expression(key1), - Expression(key2)); + Expression expression(const typename Base::ArrayNKeys& keys) const override { + return Expression(T::Measure, Expression(keys[0]), + Expression(keys[1])); + } + + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + std::vector Hs(2); + const auto &keys = Factor::keys(); + const Vector error = unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, + Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; } /// print diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 80b02404e..d9890d2ef 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -32,18 +32,18 @@ struct Range; * @addtogroup SAM */ template -class RangeFactor : public ExpressionFactor2 { +class RangeFactor : public ExpressionFactorN { private: typedef RangeFactor This; - typedef ExpressionFactor2 Base; + typedef ExpressionFactorN Base; public: /// default constructor RangeFactor() {} RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model) - : Base(key1, key2, model, measured) { - this->initialize(expression(key1, key2)); + : Base({key1, key2}, model, measured) { + this->initialize(expression({key1, key2})); } /// @return a deep copy of this factor @@ -53,11 +53,25 @@ class RangeFactor : public ExpressionFactor2 { } // Return measurement expression - Expression expression(Key key1, Key key2) const override { - Expression a1_(key1); - Expression a2_(key2); + Expression expression(const typename Base::ArrayNKeys& keys) const override { + Expression a1_(keys[0]); + Expression a2_(keys[1]); return Expression(Range(), a1_, a2_); } + + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + std::vector Hs(2); + const auto &keys = Factor::keys(); + const Vector error = Base::unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, + Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; + } /// print void print(const std::string& s = "", @@ -86,10 +100,10 @@ struct traits > */ template ::result_type> -class RangeFactorWithTransform : public ExpressionFactor2 { +class RangeFactorWithTransform : public ExpressionFactorN { private: typedef RangeFactorWithTransform This; - typedef ExpressionFactor2 Base; + typedef ExpressionFactorN Base; A1 body_T_sensor_; ///< The pose of the sensor in the body frame @@ -100,8 +114,8 @@ class RangeFactorWithTransform : public ExpressionFactor2 { RangeFactorWithTransform(Key key1, Key key2, T measured, const SharedNoiseModel& model, const A1& body_T_sensor) - : Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) { - this->initialize(expression(key1, key2)); + : Base({key1, key2}, model, measured), body_T_sensor_(body_T_sensor) { + this->initialize(expression({key1, key2})); } virtual ~RangeFactorWithTransform() {} @@ -113,15 +127,29 @@ class RangeFactorWithTransform : public ExpressionFactor2 { } // Return measurement expression - Expression expression(Key key1, Key key2) const override { + Expression expression(const typename Base::ArrayNKeys& keys) const override { Expression body_T_sensor__(body_T_sensor_); - Expression nav_T_body_(key1); + Expression nav_T_body_(keys[0]); Expression nav_T_sensor_(traits::Compose, nav_T_body_, body_T_sensor__); - Expression a2_(key2); + Expression a2_(keys[1]); return Expression(Range(), nav_T_sensor_, a2_); } + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + std::vector Hs(2); + const auto &keys = Factor::keys(); + const Vector error = Base::unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, + Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; + } + /** print contents */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { @@ -135,9 +163,12 @@ class RangeFactorWithTransform : public ExpressionFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // **IMPORTANT** We need to (de)serialize parameters before the base class, + // since it calls expression() and we need all parameters ready at that + // point. + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); ar& boost::serialization::make_nvp( "Base", boost::serialization::base_object(*this)); - ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); } }; // \ RangeFactorWithTransform diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index 12635a7e5..17a049a1d 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -68,7 +68,7 @@ TEST(BearingFactor, 2D) { values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(pointKey, Point2(-4.0, 11.0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), values, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } @@ -104,7 +104,7 @@ TEST(BearingFactor, Serialization3D) { // values.insert(poseKey, Pose3()); // values.insert(pointKey, Point3(1, 0, 0)); // -// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), // values, 1e-7, 1e-5); // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); //} diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp index 4c7a9ab91..735358d89 100644 --- a/gtsam/sam/tests/testBearingRangeFactor.cpp +++ b/gtsam/sam/tests/testBearingRangeFactor.cpp @@ -67,7 +67,7 @@ TEST(BearingRangeFactor, 2D) { values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(pointKey, Point2(-4.0, 11.0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), values, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } @@ -95,7 +95,7 @@ TEST(BearingRangeFactor, Serialization3D) { // values.insert(poseKey, Pose3()); // values.insert(pointKey, Point3(1, 0, 0)); // -// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), // values, 1e-7, 1e-5); // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); //} diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 54d4a43c0..8ae3d818b 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -141,6 +141,27 @@ TEST( RangeFactor, EqualsWithTransform ) { body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } +/* ************************************************************************* */ +TEST( RangeFactor, EqualsAfterDeserializing) { + // Check that the same results are obtained after deserializing: + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + + RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, + body_P_sensor_3D), factor3D_2; + + // check with Equal() trait: + gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2); + CHECK(assert_equal(factor3D_1, factor3D_2)); + + const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0)); + const Point3 point(-2.0, 11.0, 1.0); + const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}; + + const Vector error_1 = factor3D_1.unwhitenedError(values); + const Vector error_2 = factor3D_2.unwhitenedError(values); + CHECK(assert_equal(error_1, error_2)); +} /* ************************************************************************* */ TEST( RangeFactor, Error2D ) { @@ -152,7 +173,7 @@ TEST( RangeFactor, Error2D ) { Point2 point(-4.0, 11.0); // Use the factor to calculate the error - Vector actualError(factor.evaluateError(pose, point)); + Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}})); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance Vector expectedError = (Vector(1) << 0.295630141).finished(); @@ -175,7 +196,7 @@ TEST( RangeFactor, Error2DWithTransform ) { Point2 point(-4.0, 11.0); // Use the factor to calculate the error - Vector actualError(factor.evaluateError(pose, point)); + Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}})); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance Vector expectedError = (Vector(1) << 0.295630141).finished(); @@ -194,7 +215,7 @@ TEST( RangeFactor, Error3D ) { Point3 point(-2.0, 11.0, 1.0); // Use the factor to calculate the error - Vector actualError(factor.evaluateError(pose, point)); + Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}})); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance Vector expectedError = (Vector(1) << 0.295630141).finished(); @@ -218,7 +239,7 @@ TEST( RangeFactor, Error3DWithTransform ) { Point3 point(-2.0, 11.0, 1.0); // Use the factor to calculate the error - Vector actualError(factor.evaluateError(pose, point)); + Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}})); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance Vector expectedError = (Vector(1) << 0.295630141).finished(); @@ -266,8 +287,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { Point2 point(-4.0, 11.0); // Use the factor to calculate the Jacobians - Matrix H1Actual, H2Actual; - factor.evaluateError(pose, point, H1Actual, H2Actual); + std::vector actualHs(2); + factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; @@ -291,8 +314,10 @@ TEST( RangeFactor, Jacobian3D ) { Point3 point(-2.0, 11.0, 1.0); // Use the factor to calculate the Jacobians - Matrix H1Actual, H2Actual; - factor.evaluateError(pose, point, H1Actual, H2Actual); + std::vector actualHs(2); + factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; @@ -321,8 +346,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { Point3 point(-2.0, 11.0, 1.0); // Use the factor to calculate the Jacobians - Matrix H1Actual, H2Actual; - factor.evaluateError(pose, point, H1Actual, H2Actual); + std::vector actualHs(2); + factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; @@ -350,7 +377,7 @@ TEST(RangeFactor, Point3) { Vector expectedError = (Vector(1) << 0.295630141).finished(); // Verify we get the expected error - CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9)); + CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9)); } /* ************************************************************************* */ @@ -393,7 +420,7 @@ TEST(RangeFactor, NonGTSAM) { Vector expectedError = (Vector(1) << 0.295630141).finished(); // Verify we get the expected error - CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9)); + CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9)); } /* ************************************************************************* */ diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index d33c7ba1d..e3e37e7c7 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -630,6 +630,103 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); } + +/* ************************************************************************* */ +// Test N-ary variadic template +class TestNaryFactor + : public gtsam::ExpressionFactorN { +private: + using This = TestNaryFactor; + using Base = + gtsam::ExpressionFactorN; + +public: + /// default constructor + TestNaryFactor() = default; + ~TestNaryFactor() override = default; + + TestNaryFactor(gtsam::Key kR1, gtsam::Key kV1, gtsam::Key kR2, gtsam::Key kV2, + const gtsam::SharedNoiseModel &model, const gtsam::Point3& measured) + : Base({kR1, kV1, kR2, kV2}, model, measured) { + this->initialize(expression({kR1, kV1, kR2, kV2})); + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + // Return measurement expression + gtsam::Expression expression( + const std::array &keys) const override { + gtsam::Expression R1_(keys[0]); + gtsam::Expression V1_(keys[1]); + gtsam::Expression R2_(keys[2]); + gtsam::Expression V2_(keys[3]); + return {gtsam::rotate(R1_, V1_) - gtsam::rotate(R2_, V2_)}; + } + + /** print */ + void print(const std::string &s, + const gtsam::KeyFormatter &keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "TestNaryFactor(" + << keyFormatter(Factor::keys_[0]) << "," + << keyFormatter(Factor::keys_[1]) << "," + << keyFormatter(Factor::keys_[2]) << "," + << keyFormatter(Factor::keys_[3]) << ")\n"; + gtsam::traits::Print(measured_, " measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + bool equals(const gtsam::NonlinearFactor &expected, + double tol = 1e-9) const override { + const This *e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + gtsam::traits::Equals(measured_,e->measured_, tol); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &boost::serialization::make_nvp( + "TestNaryFactor", + boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(measured_); + } +}; + +TEST(ExpressionFactor, variadicTemplate) { + using gtsam::symbol_shorthand::R; + using gtsam::symbol_shorthand::V; + + // Create factor + TestNaryFactor f(R(0),V(0), R(1), V(1), noiseModel::Unit::Create(3), Point3(0,0,0)); + + // Create some values + Values values; + values.insert(R(0), Rot3::Ypr(0.1, 0.2, 0.3)); + values.insert(V(0), Point3(1, 2, 3)); + values.insert(R(1), Rot3::Ypr(0.2, 0.5, 0.2)); + values.insert(V(1), Point3(5, 6, 7)); + + // Check unwhitenedError + std::vector H(4); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(4, H.size()); + EXPECT(assert_equal(Eigen::Vector3d(-5.63578115, -4.85353243, -1.4801204), actual, 1e-5)); + + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5); +} + + /* ************************************************************************* */ int main() { TestResult tr;